mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 05:07:36 +08:00
module_base: remove CRTP template pattern to reduce flash bloat (#26476)
* module_base: claude rewrite to remove CRTP bloat * module_base: apply to all drivers/modules * format * fix build errors * fix missing syntax * remove reference to module.h in files that need module_base.h * remove old ModuleBase<T> * add module_base.cpp to px4_protected_layers.cmake * fix IridiumSBD can_stop() * fix IridiumSBD.cpp * clang-tidy: downcast static cast * get_instance() template accessor, revert clang-tidy global * rename module_base.h to module.h * revert changes in zenoh/Kconfig.topics
This commit is contained in:
@@ -43,6 +43,8 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_io_heater.h>
|
||||
|
||||
ModuleBase::Descriptor Core_Heater::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
# ifndef GPIO_CORE_HEATER_OUTPUT
|
||||
# error "To use the heater driver, the board_config.h must define and initialize GPIO_CORE_HEATER_OUTPUT"
|
||||
# endif
|
||||
@@ -62,7 +64,7 @@ Core_Heater::~Core_Heater()
|
||||
int Core_Heater::custom_command(int argc, char *argv[])
|
||||
{
|
||||
// Check if the driver is running.
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("not running");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -117,7 +119,7 @@ bool Core_Heater::initialize_topics()
|
||||
void Core_Heater::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -216,8 +218,8 @@ int Core_Heater::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(core_heater);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(core_heater);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
core_heater->start();
|
||||
return 0;
|
||||
@@ -257,5 +259,5 @@ Background process running periodically on the LP work queue to regulate IMU tem
|
||||
|
||||
extern "C" __EXPORT int core_heater_main(int argc, char *argv[])
|
||||
{
|
||||
return Core_Heater::main(argc, argv);
|
||||
return ModuleBase::main(Core_Heater::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -56,9 +56,11 @@ using namespace time_literals;
|
||||
#define CONTROLLER_PERIOD_DEFAULT 10000
|
||||
#define TEMPERATURE_TARGET_THRESHOLD 2.5f
|
||||
|
||||
class Core_Heater : public ModuleBase<Core_Heater>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class Core_Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
Core_Heater();
|
||||
|
||||
virtual ~Core_Heater();
|
||||
|
||||
@@ -33,6 +33,8 @@
|
||||
|
||||
#include "NavioRGBLed.hpp"
|
||||
|
||||
ModuleBase::Descriptor NavioRGBLed::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
NavioRGBLed::NavioRGBLed() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
|
||||
{
|
||||
@@ -130,8 +132,8 @@ int NavioRGBLed::task_spawn(int argc, char *argv[])
|
||||
NavioRGBLed *instance = new NavioRGBLed();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -142,8 +144,8 @@ int NavioRGBLed::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -170,5 +172,5 @@ Emlid Navio2 RGB LED driver.
|
||||
|
||||
extern "C" __EXPORT int navio_rgbled_main(int argc, char *argv[])
|
||||
{
|
||||
return NavioRGBLed::main(argc, argv);
|
||||
return ModuleBase::main(NavioRGBLed::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -40,9 +40,11 @@
|
||||
|
||||
#include <lib/led/led.h>
|
||||
|
||||
class NavioRGBLed : public ModuleBase<NavioRGBLed>, public px4::ScheduledWorkItem
|
||||
class NavioRGBLed : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
NavioRGBLed();
|
||||
~NavioRGBLed() override;
|
||||
|
||||
|
||||
@@ -49,6 +49,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor GhstRc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
uint32_t GhstRc::baudrate = GHST_BAUDRATE;
|
||||
|
||||
GhstRc::GhstRc(const char *device) :
|
||||
@@ -114,8 +116,8 @@ int GhstRc::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
@@ -174,7 +176,7 @@ void GhstRc::Run()
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
_rc_fd = -1;
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -308,5 +310,5 @@ This module parses the GHST RC uplink protocol and can generate GHST downlink te
|
||||
|
||||
extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return GhstRc::main(argc, argv);
|
||||
return ModuleBase::main(GhstRc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -54,9 +54,11 @@
|
||||
|
||||
#define GHST_MAX_NUM_CHANNELS (16)
|
||||
|
||||
class GhstRc : public ModuleBase<GhstRc>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
GhstRc(const char *device);
|
||||
~GhstRc() override;
|
||||
|
||||
|
||||
@@ -59,6 +59,8 @@
|
||||
|
||||
#include "rc_controller.hpp"
|
||||
|
||||
ModuleBase::Descriptor RC_ControllerModule::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
int RC_ControllerModule::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
@@ -69,35 +71,35 @@ int RC_ControllerModule::print_status()
|
||||
|
||||
int RC_ControllerModule::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
print_usage("not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "throttle")) {
|
||||
uint16_t val = atoi(argv[1]);
|
||||
get_instance()->set_throttle(val);
|
||||
get_instance<RC_ControllerModule>(desc)->set_throttle(val);
|
||||
PX4_INFO("Setting throttle to %u", val);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "yaw")) {
|
||||
uint16_t val = atoi(argv[1]);
|
||||
get_instance()->set_yaw(val);
|
||||
get_instance<RC_ControllerModule>(desc)->set_yaw(val);
|
||||
PX4_INFO("Setting yaw to %u", val);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "pitch")) {
|
||||
uint16_t val = atoi(argv[1]);
|
||||
get_instance()->set_pitch(val);
|
||||
get_instance<RC_ControllerModule>(desc)->set_pitch(val);
|
||||
PX4_INFO("Setting pitch to %u", val);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "roll")) {
|
||||
uint16_t val = atoi(argv[1]);
|
||||
get_instance()->set_roll(val);
|
||||
get_instance<RC_ControllerModule>(desc)->set_roll(val);
|
||||
PX4_INFO("Setting roll to %u", val);
|
||||
return 0;
|
||||
}
|
||||
@@ -106,17 +108,24 @@ int RC_ControllerModule::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
|
||||
int RC_ControllerModule::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return RC_ControllerModule::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int RC_ControllerModule::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("RC_ControllerModule",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1024,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
desc.task_id = px4_task_spawn_cmd("RC_ControllerModule",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1024,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
if (desc.task_id < 0) {
|
||||
desc.task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
@@ -252,5 +261,5 @@ int RC_ControllerModule::print_usage(const char *reason)
|
||||
|
||||
int rc_controller_main(int argc, char *argv[])
|
||||
{
|
||||
return RC_ControllerModule::main(argc, argv);
|
||||
return ModuleBase::main(RC_ControllerModule::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -41,9 +41,11 @@
|
||||
|
||||
extern "C" __EXPORT int rc_controller_main(int argc, char *argv[]);
|
||||
|
||||
class RC_ControllerModule : public ModuleBase<RC_ControllerModule>, public ModuleParams
|
||||
class RC_ControllerModule : public ModuleBase, public ModuleParams
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
RC_ControllerModule();
|
||||
|
||||
virtual ~RC_ControllerModule() = default;
|
||||
@@ -51,6 +53,9 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static RC_ControllerModule *instantiate(int argc, char *argv[]);
|
||||
|
||||
|
||||
@@ -41,6 +41,8 @@
|
||||
|
||||
using namespace std;
|
||||
|
||||
ModuleBase::Descriptor VoxlSaveCalParams::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
static bool debug = false;
|
||||
|
||||
VoxlSaveCalParams::VoxlSaveCalParams() :
|
||||
@@ -145,7 +147,7 @@ VoxlSaveCalParams::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_parameter_primary_set_value_request_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -186,8 +188,8 @@ int VoxlSaveCalParams::task_spawn(int argc, char *argv[])
|
||||
VoxlSaveCalParams *instance = new VoxlSaveCalParams();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -198,8 +200,8 @@ int VoxlSaveCalParams::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -230,5 +232,5 @@ This implements autosaving of calibration parameters on VOXL2 platform.
|
||||
|
||||
extern "C" __EXPORT int voxl_save_cal_params_main(int argc, char *argv[])
|
||||
{
|
||||
return VoxlSaveCalParams::main(argc, argv);
|
||||
return ModuleBase::main(VoxlSaveCalParams::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -46,10 +46,12 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class VoxlSaveCalParams : public ModuleBase<VoxlSaveCalParams>, public ModuleParams,
|
||||
class VoxlSaveCalParams : public ModuleBase, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
VoxlSaveCalParams();
|
||||
~VoxlSaveCalParams() = default;
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2017-2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -33,23 +33,27 @@
|
||||
|
||||
/**
|
||||
* @file module.h
|
||||
*
|
||||
* Non-template base class for modules. Replaces the CRTP ModuleBase<T>
|
||||
* pattern with a descriptor-based approach that shares a single copy of the
|
||||
* common static methods (main, start, stop, status, etc.) across all modules.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <pthread.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include <pthread.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include <cstring>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
|
||||
/**
|
||||
* @brief This mutex protects against race conditions during startup & shutdown of modules.
|
||||
@@ -61,527 +65,153 @@ extern pthread_mutex_t px4_modules_mutex;
|
||||
|
||||
/**
|
||||
* @class ModuleBase
|
||||
* Base class for modules, implementing common functionality,
|
||||
* Non-template base class for modules, implementing common functionality
|
||||
* such as 'start', 'stop' and 'status' commands.
|
||||
* Currently does not support modules which allow multiple instances,
|
||||
* such as mavlink.
|
||||
*
|
||||
* The class is implemented as curiously recurring template pattern (CRTP).
|
||||
* It allows to have a static object in the base class that is different for
|
||||
* each module type, and call static methods from the base class.
|
||||
*
|
||||
* @note Required methods for a derived class:
|
||||
* When running in its own thread:
|
||||
* static int task_spawn(int argc, char *argv[])
|
||||
* {
|
||||
* // call px4_task_spawn_cmd() with &run_trampoline as startup method
|
||||
* // optional: wait until _object is not null, which means the task got initialized (use a timeout)
|
||||
* // set _task_id and return 0
|
||||
* // on error return != 0 (and _task_id must be -1)
|
||||
* }
|
||||
*
|
||||
* static T *instantiate(int argc, char *argv[])
|
||||
* {
|
||||
* // this is called from within the new thread, from run_trampoline()
|
||||
* // parse the arguments
|
||||
* // create a new object T & return it
|
||||
* // or return nullptr on error
|
||||
* }
|
||||
*
|
||||
* static int custom_command(int argc, char *argv[])
|
||||
* {
|
||||
* // support for custom commands
|
||||
* // if none are supported, just do:
|
||||
* return print_usage("unrecognized command");
|
||||
* }
|
||||
*
|
||||
* static int print_usage(const char *reason = nullptr)
|
||||
* {
|
||||
* // use the PRINT_MODULE_* methods...
|
||||
* }
|
||||
*
|
||||
* When running on the work queue:
|
||||
* - custom_command & print_usage as above
|
||||
* static int task_spawn(int argc, char *argv[]) {
|
||||
* // parse the arguments
|
||||
* // set _object (here or from the work_queue() callback)
|
||||
* // call work_queue() (with a custom cycle trampoline)
|
||||
* // optional: wait until _object is not null, which means the task got initialized (use a timeout)
|
||||
* // set _task_id to task_id_is_work_queue and return 0
|
||||
* // on error return != 0 (and _task_id must be -1)
|
||||
* }
|
||||
* Each module provides a static Descriptor instance that holds
|
||||
* module-specific function pointers and per-module storage (object
|
||||
* pointer and task ID). The shared static methods operate on the
|
||||
* descriptor instead of CRTP template statics.
|
||||
*/
|
||||
template<class T>
|
||||
class ModuleBase
|
||||
{
|
||||
public:
|
||||
ModuleBase() : _task_should_exit{false} {}
|
||||
virtual ~ModuleBase() {}
|
||||
/**
|
||||
* Per-module descriptor providing module-specific function pointers
|
||||
* and storage (object pointer, task ID).
|
||||
*/
|
||||
struct Descriptor {
|
||||
Descriptor(int (*ts)(int, char **), int (*cc)(int, char **), int (*pu)(const char *))
|
||||
: task_spawn(ts), custom_command(cc), print_usage(pu) {}
|
||||
|
||||
int (*task_spawn)(int argc, char *argv[]);
|
||||
int (*custom_command)(int argc, char *argv[]);
|
||||
int (*print_usage)(const char *reason);
|
||||
px4::atomic<ModuleBase *> object{nullptr};
|
||||
int task_id{-1};
|
||||
};
|
||||
|
||||
ModuleBase() = default;
|
||||
virtual ~ModuleBase() = default;
|
||||
|
||||
/**
|
||||
* @brief main Main entry point to the module that should be
|
||||
* called directly from the module's main method.
|
||||
* @param argc The task argument count.
|
||||
* @param argc Pointer to the task argument variable array.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Main entry point to the module that should be called directly
|
||||
* from the module's main method.
|
||||
*/
|
||||
static int main(int argc, char *argv[])
|
||||
{
|
||||
if (argc <= 1 ||
|
||||
strcmp(argv[1], "-h") == 0 ||
|
||||
strcmp(argv[1], "help") == 0 ||
|
||||
strcmp(argv[1], "info") == 0 ||
|
||||
strcmp(argv[1], "usage") == 0) {
|
||||
return T::print_usage();
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "start") == 0) {
|
||||
// Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.
|
||||
return start_command_base(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "status") == 0) {
|
||||
return status_command();
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "stop") == 0) {
|
||||
return stop_command();
|
||||
}
|
||||
|
||||
lock_module(); // Lock here, as the method could access _object.
|
||||
int ret = T::custom_command(argc - 1, argv + 1);
|
||||
unlock_module();
|
||||
|
||||
return ret;
|
||||
}
|
||||
static int main(Descriptor &desc, int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Entry point for px4_task_spawn_cmd() if the module runs in its own thread.
|
||||
* It does:
|
||||
* - instantiate the object
|
||||
* - call run() on it to execute the main loop
|
||||
* - cleanup: delete the object
|
||||
* @param argc The task argument count.
|
||||
* @param argc Pointer to the task argument variable array.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Start command: checks if running, calls desc.task_spawn().
|
||||
*/
|
||||
static int run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
// We don't need the task name at this point.
|
||||
argc -= 1;
|
||||
argv += 1;
|
||||
|
||||
T *object = T::instantiate(argc, argv);
|
||||
_object.store(object);
|
||||
|
||||
if (object) {
|
||||
object->run();
|
||||
|
||||
} else {
|
||||
PX4_ERR("failed to instantiate object");
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
exit_and_cleanup();
|
||||
|
||||
return ret;
|
||||
}
|
||||
static int start_command(Descriptor &desc, int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Stars the command, ('command start'), checks if if is already
|
||||
* running and calls T::task_spawn() if it's not.
|
||||
* @param argc The task argument count.
|
||||
* @param argc Pointer to the task argument variable array.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Stop command: request stop and wait for exit.
|
||||
*/
|
||||
static int start_command_base(int argc, char *argv[])
|
||||
{
|
||||
int ret = 0;
|
||||
lock_module();
|
||||
|
||||
if (is_running()) {
|
||||
ret = -1;
|
||||
PX4_ERR("Task already running");
|
||||
|
||||
} else {
|
||||
ret = T::task_spawn(argc, argv);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("Task start failed (%i)", ret);
|
||||
}
|
||||
}
|
||||
|
||||
unlock_module();
|
||||
return ret;
|
||||
}
|
||||
static int stop_command(Descriptor &desc);
|
||||
|
||||
/**
|
||||
* @brief Stops the command, ('command stop'), checks if it is running and if it is, request the module to stop
|
||||
* and waits for the task to complete.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Status command: call print_status() on running instance.
|
||||
*/
|
||||
static int stop_command()
|
||||
{
|
||||
int ret = 0;
|
||||
lock_module();
|
||||
|
||||
if (is_running()) {
|
||||
T *object = _object.load();
|
||||
|
||||
if (object) {
|
||||
object->request_stop();
|
||||
|
||||
unsigned int i = 0;
|
||||
|
||||
do {
|
||||
unlock_module();
|
||||
px4_usleep(10000); // 10 ms
|
||||
lock_module();
|
||||
|
||||
if (++i > 500 && _task_id != -1) { // wait at most 5 sec
|
||||
PX4_ERR("timeout, forcing stop");
|
||||
|
||||
if (_task_id != task_id_is_work_queue) {
|
||||
px4_task_delete(_task_id);
|
||||
}
|
||||
|
||||
_task_id = -1;
|
||||
|
||||
delete _object.load();
|
||||
_object.store(nullptr);
|
||||
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
} while (_task_id != -1);
|
||||
|
||||
} else {
|
||||
// In the very unlikely event that can only happen on work queues,
|
||||
// if the starting thread does not wait for the work queue to initialize,
|
||||
// and inside the work queue, the allocation of _object fails
|
||||
// and exit_and_cleanup() is not called, set the _task_id as invalid.
|
||||
_task_id = -1;
|
||||
}
|
||||
}
|
||||
|
||||
unlock_module();
|
||||
return ret;
|
||||
}
|
||||
static int status_command(Descriptor &desc);
|
||||
|
||||
/**
|
||||
* @brief Handle 'command status': check if running and call print_status() if it is
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Cleanup: delete object, reset task_id. Called from module thread.
|
||||
*/
|
||||
static int status_command()
|
||||
{
|
||||
int ret = -1;
|
||||
lock_module();
|
||||
|
||||
if (is_running() && _object.load()) {
|
||||
T *object = _object.load();
|
||||
ret = object->print_status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("not running");
|
||||
}
|
||||
|
||||
unlock_module();
|
||||
return ret;
|
||||
}
|
||||
static void exit_and_cleanup(Descriptor &desc);
|
||||
|
||||
/**
|
||||
* @brief Print the status if the module is running. This can be overridden by the module to provide
|
||||
* more specific information.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
* @brief Check if the module is running.
|
||||
*/
|
||||
virtual int print_status()
|
||||
{
|
||||
PX4_INFO("running");
|
||||
return 0;
|
||||
}
|
||||
static bool is_running(const Descriptor &desc) { return desc.task_id != -1; }
|
||||
|
||||
/**
|
||||
* @brief Main loop method for modules running in their own thread. Called from run_trampoline().
|
||||
* This method must return when should_exit() returns true.
|
||||
* @brief Wait until the object is initialized (called from task_spawn).
|
||||
*/
|
||||
virtual void run()
|
||||
{
|
||||
}
|
||||
static int wait_until_running(Descriptor &desc, int timeout_ms = 1000);
|
||||
|
||||
/**
|
||||
* @brief Returns the status of the module.
|
||||
* @return Returns true if the module is running, false otherwise.
|
||||
* @brief Shared run-trampoline for thread-based modules.
|
||||
*
|
||||
* Each thread-based module provides a thin per-module trampoline
|
||||
* that calls this with its descriptor + instantiate function.
|
||||
*/
|
||||
static bool is_running()
|
||||
using instantiate_fn = ModuleBase * (*)(int argc, char *argv[]);
|
||||
static int run_trampoline_impl(Descriptor &desc, instantiate_fn instantiate,
|
||||
int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Main loop method for modules running in their own thread.
|
||||
*/
|
||||
virtual void run() {}
|
||||
|
||||
/**
|
||||
* @brief Print the status of the module.
|
||||
*/
|
||||
virtual int print_status();
|
||||
|
||||
/**
|
||||
* @brief Tells the module to stop.
|
||||
*/
|
||||
virtual void request_stop() { _task_should_exit.store(true); }
|
||||
|
||||
/**
|
||||
* @brief Checks if the module should stop.
|
||||
*/
|
||||
bool should_exit() const { return _task_should_exit.load(); }
|
||||
|
||||
/**
|
||||
* @brief Typed accessor for the running module instance.
|
||||
* Centralizes the ModuleBase* -> T* downcast in one place.
|
||||
*/
|
||||
template<typename T>
|
||||
static T *get_instance(Descriptor &desc)
|
||||
{
|
||||
return _task_id != -1;
|
||||
return static_cast<T *>(desc.object.load()); // NOLINT(cppcoreguidelines-pro-type-static-cast-downcast)
|
||||
}
|
||||
|
||||
static constexpr int task_id_is_work_queue = -2;
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Tells the module to stop (used from outside or inside the module thread).
|
||||
*/
|
||||
virtual void request_stop()
|
||||
{
|
||||
_task_should_exit.store(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if the module should stop (used within the module thread).
|
||||
* @return Returns True iff successful, false otherwise.
|
||||
*/
|
||||
bool should_exit() const
|
||||
{
|
||||
return _task_should_exit.load();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Exits the module and delete the object. Called from within the module's thread.
|
||||
* For work queue modules, this needs to be called from the derived class in the
|
||||
* cycle method, when should_exit() returns true.
|
||||
*/
|
||||
static void exit_and_cleanup()
|
||||
{
|
||||
// Take the lock here:
|
||||
// - if startup fails and we're faster than the parent thread, it will set
|
||||
// _task_id and subsequently it will look like the task is running.
|
||||
// - deleting the object must take place inside the lock.
|
||||
lock_module();
|
||||
|
||||
delete _object.load();
|
||||
_object.store(nullptr);
|
||||
|
||||
_task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue.
|
||||
unlock_module();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn().
|
||||
* @return Returns 0 iff successful, -1 on timeout or otherwise.
|
||||
*/
|
||||
static int wait_until_running(int timeout_ms = 1000)
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
do {
|
||||
px4_usleep(2000);
|
||||
|
||||
} while (!_object.load() && ++i < timeout_ms / 2);
|
||||
|
||||
if (i >= timeout_ms / 2) {
|
||||
PX4_ERR("Timed out while waiting for thread to start");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the module's object instance, (this is null if it's not running).
|
||||
*/
|
||||
static T *get_instance()
|
||||
{
|
||||
return (T *)_object.load();
|
||||
}
|
||||
|
||||
/**
|
||||
* @var _object Instance if the module is running.
|
||||
* @note There will be one instance for each template type.
|
||||
*/
|
||||
static px4::atomic<T *> _object;
|
||||
|
||||
/** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */
|
||||
static int _task_id;
|
||||
|
||||
/** @var task_id_is_work_queue Value to indicate if the task runs on the work queue. */
|
||||
static constexpr const int task_id_is_work_queue = -2;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief lock_module Mutex to lock the module thread.
|
||||
*/
|
||||
static void lock_module()
|
||||
{
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief unlock_module Mutex to unlock the module thread.
|
||||
*/
|
||||
static void unlock_module()
|
||||
{
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
}
|
||||
|
||||
/** @var _task_should_exit Boolean flag to indicate if the task should exit. */
|
||||
px4::atomic_bool _task_should_exit{false};
|
||||
};
|
||||
|
||||
template<class T>
|
||||
px4::atomic<T *> ModuleBase<T>::_object{nullptr};
|
||||
|
||||
template<class T>
|
||||
int ModuleBase<T>::_task_id = -1;
|
||||
|
||||
|
||||
#endif /* __cplusplus */
|
||||
|
||||
|
||||
/* -------- C-linkage declarations for PRINT_MODULE_* helpers -------- */
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* @brief Module documentation and command usage help methods.
|
||||
* These are extracted with the Tools/px_process_module_doc.py
|
||||
* script and must be kept in sync.
|
||||
*/
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
/**
|
||||
* @note Disable module description on NuttX to reduce Flash usage.
|
||||
* There's a GCC bug (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=55971), preventing us to use
|
||||
* a macro, but GCC will remove the string as well with this empty inline method.
|
||||
* @param description The provided functionality of the module and potentially the most important parameters.
|
||||
*/
|
||||
static inline void PRINT_MODULE_DESCRIPTION(const char *description) {}
|
||||
#else
|
||||
|
||||
/**
|
||||
* @brief Prints module documentation (will also be used for online documentation). It uses Markdown syntax
|
||||
* and should include these sections:
|
||||
* - ### Description
|
||||
* Provided functionality of the module and potentially the most important parameters.
|
||||
* - ### Implementation
|
||||
* High-level implementation overview
|
||||
* - ### Examples
|
||||
* Examples how to use the CLI interface (if it's non-trivial)
|
||||
*
|
||||
* In addition to the Markdown syntax, a line beginning with '$ ' can be used to mark a command:
|
||||
* $ module start -p param
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_DESCRIPTION(const char *description);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Prints the command name.
|
||||
* @param executable_name: command name used in scripts & CLI
|
||||
* @param category one of: driver, estimator, controller, system, communication, command, template
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_NAME(const char *executable_name, const char *category);
|
||||
|
||||
/**
|
||||
* @brief Specify a subcategory (optional).
|
||||
* @param subcategory e.g. if the category is 'driver', subcategory can be 'distance_sensor'
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory);
|
||||
|
||||
/**
|
||||
* @brief Prints the name for a command without any sub-commands (@see PRINT_MODULE_USAGE_NAME()).
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_NAME_SIMPLE(const char *executable_name, const char *category);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Prints a command with a short description what it does.
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_COMMAND_DESCR(const char *name, const char *description);
|
||||
|
||||
#define PRINT_MODULE_USAGE_COMMAND(name) \
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR(name, NULL);
|
||||
|
||||
/**
|
||||
* @brief Prints the default commands: stop & status.
|
||||
*/
|
||||
#define PRINT_MODULE_USAGE_DEFAULT_COMMANDS() \
|
||||
PRINT_MODULE_USAGE_COMMAND("stop"); \
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "print status info");
|
||||
|
||||
/**
|
||||
* Print default params for I2C or SPI drivers
|
||||
* @param i2c_support true if the driver supports I2C
|
||||
* @param spi_support true if the driver supports SPI
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(bool i2c_support, bool spi_support);
|
||||
|
||||
/**
|
||||
* Configurable I2C address (via -a <address>)
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(uint8_t default_address);
|
||||
|
||||
/**
|
||||
* -k flag
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(void);
|
||||
|
||||
/** @note Each of the PRINT_MODULE_USAGE_PARAM_* methods apply to the previous PRINT_MODULE_USAGE_COMMAND_DESCR(). */
|
||||
|
||||
/**
|
||||
* @brief Prints an integer parameter.
|
||||
* @param option_char The option character.
|
||||
* @param default_val The parameter default value (set to -1 if not applicable).
|
||||
* @param min_val The parameter minimum value.
|
||||
* @param max_val The parameter value.
|
||||
* @param description Pointer to the usage description.
|
||||
* @param is_optional true if this parameter is optional
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val, int max_val,
|
||||
const char *description, bool is_optional);
|
||||
|
||||
/**
|
||||
* @brief Prints a float parameter.
|
||||
* @note See PRINT_MODULE_USAGE_PARAM_INT().
|
||||
* @param default_val The parameter default value (set to NaN if not applicable).
|
||||
* @param min_val The parameter minimum value.
|
||||
* @param max_val The parameter maximum value.
|
||||
* @param description Pointer to the usage description. Pointer to the usage description.
|
||||
* @param is_optional true if this parameter is optional
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float min_val, float max_val,
|
||||
const char *description, bool is_optional);
|
||||
|
||||
/**
|
||||
* @brief Prints a flag parameter, without any value.
|
||||
* @note See PRINT_MODULE_USAGE_PARAM_INT().
|
||||
* @param option_char The option character.
|
||||
* @param description Pointer to the usage description.
|
||||
* @param is_optional true if this parameter is optional
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAM_FLAG(char option_char, const char *description, bool is_optional);
|
||||
|
||||
/**
|
||||
* @brief Prints a string parameter.
|
||||
* @param option_char The option character.
|
||||
* @param default_val The default value, can be nullptr.
|
||||
* @param values The valid values, it has one of the following forms:
|
||||
* - nullptr: leave unspecified, or any value is valid
|
||||
* - "<file>" or "<file:dev>": a file or more specifically a device file (eg. serial device)
|
||||
* - "<topic_name>": uORB topic name
|
||||
* - "<value1> [<value2>]": a list of values
|
||||
* - "on|off": a concrete set of valid strings separated by "|".
|
||||
* @param description Pointer to the usage description.
|
||||
* @param is_optional True iff this parameter is optional.
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, const char *values,
|
||||
const char *description, bool is_optional);
|
||||
|
||||
/**
|
||||
* @brief Prints a comment, that applies to the next arguments or parameters. For example to indicate that
|
||||
* a parameter applies to several or all commands.
|
||||
* @param comment
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_PARAM_COMMENT(const char *comment);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Prints the definition for an argument, which does not have the typical -p <val> form,
|
||||
* but for example 'param set <param> <value>'
|
||||
* @param values eg. "<file>", "<param> <value>" or "<value1> [<value2>]"
|
||||
* @param description Pointer to the usage description.
|
||||
* @param is_optional true if this parameter is optional
|
||||
*/
|
||||
__EXPORT void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is_optional);
|
||||
|
||||
|
||||
__END_DECLS
|
||||
|
||||
@@ -0,0 +1,229 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file module_base.cpp
|
||||
*
|
||||
* Shared implementations of ModuleBase static methods. One copy linked
|
||||
* for all modules, replacing per-module CRTP template instantiations.
|
||||
*/
|
||||
|
||||
#ifndef MODULE_NAME
|
||||
#define MODULE_NAME "module"
|
||||
#endif
|
||||
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include <pthread.h>
|
||||
#include <cstring>
|
||||
|
||||
extern pthread_mutex_t px4_modules_mutex;
|
||||
|
||||
int ModuleBase::print_status()
|
||||
{
|
||||
printf("running\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ModuleBase::main(Descriptor &desc, int argc, char *argv[])
|
||||
{
|
||||
if (argc <= 1 ||
|
||||
strcmp(argv[1], "-h") == 0 ||
|
||||
strcmp(argv[1], "help") == 0 ||
|
||||
strcmp(argv[1], "info") == 0 ||
|
||||
strcmp(argv[1], "usage") == 0) {
|
||||
return desc.print_usage(nullptr);
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "start") == 0) {
|
||||
// Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.
|
||||
return start_command(desc, argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "status") == 0) {
|
||||
return status_command(desc);
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "stop") == 0) {
|
||||
return stop_command(desc);
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
int ret = desc.custom_command(argc - 1, argv + 1);
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ModuleBase::start_command(Descriptor &desc, int argc, char *argv[])
|
||||
{
|
||||
int ret = 0;
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
|
||||
if (is_running(desc)) {
|
||||
ret = -1;
|
||||
PX4_ERR("Task already running");
|
||||
|
||||
} else {
|
||||
ret = desc.task_spawn(argc, argv);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("Task start failed (%i)", ret);
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ModuleBase::stop_command(Descriptor &desc)
|
||||
{
|
||||
int ret = 0;
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
|
||||
if (is_running(desc)) {
|
||||
ModuleBase *object = desc.object.load();
|
||||
|
||||
if (object) {
|
||||
object->request_stop();
|
||||
|
||||
unsigned int i = 0;
|
||||
|
||||
do {
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
px4_usleep(10000); // 10 ms
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
|
||||
if (++i > 500 && desc.task_id != -1) { // wait at most 5 sec
|
||||
PX4_ERR("timeout, forcing stop");
|
||||
|
||||
if (desc.task_id != task_id_is_work_queue) {
|
||||
px4_task_delete(desc.task_id);
|
||||
}
|
||||
|
||||
desc.task_id = -1;
|
||||
|
||||
delete desc.object.load();
|
||||
desc.object.store(nullptr);
|
||||
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
} while (desc.task_id != -1);
|
||||
|
||||
} else {
|
||||
// In the very unlikely event that can only happen on work queues,
|
||||
// if the starting thread does not wait for the work queue to initialize,
|
||||
// and inside the work queue, the allocation of _object fails
|
||||
// and exit_and_cleanup() is not called, set the task_id as invalid.
|
||||
desc.task_id = -1;
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int ModuleBase::status_command(Descriptor &desc)
|
||||
{
|
||||
int ret = -1;
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
|
||||
if (is_running(desc) && desc.object.load()) {
|
||||
ModuleBase *object = desc.object.load();
|
||||
ret = object->print_status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("not running");
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
return ret;
|
||||
}
|
||||
|
||||
void ModuleBase::exit_and_cleanup(Descriptor &desc)
|
||||
{
|
||||
// Take the lock here:
|
||||
// - if startup fails and we're faster than the parent thread, it will set
|
||||
// task_id and subsequently it will look like the task is running.
|
||||
// - deleting the object must take place inside the lock.
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
|
||||
delete desc.object.load();
|
||||
desc.object.store(nullptr);
|
||||
|
||||
desc.task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue.
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
}
|
||||
|
||||
int ModuleBase::wait_until_running(Descriptor &desc, int timeout_ms)
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
do {
|
||||
px4_usleep(2000);
|
||||
|
||||
} while (!desc.object.load() && ++i < timeout_ms / 2);
|
||||
|
||||
if (i >= timeout_ms / 2) {
|
||||
PX4_ERR("Timed out while waiting for thread to start");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ModuleBase::run_trampoline_impl(Descriptor &desc, instantiate_fn instantiate,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
// We don't need the task name at this point.
|
||||
argc -= 1;
|
||||
argv += 1;
|
||||
|
||||
ModuleBase *object = instantiate(argc, argv);
|
||||
desc.object.store(object);
|
||||
|
||||
if (object) {
|
||||
object->run();
|
||||
|
||||
} else {
|
||||
PX4_ERR("failed to instantiate object");
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
exit_and_cleanup(desc);
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -3,6 +3,7 @@
|
||||
add_library(px4_layer
|
||||
${KERNEL_SRCS}
|
||||
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
|
||||
@@ -5,6 +5,7 @@ add_library(px4_layer
|
||||
board_dma_alloc.c
|
||||
board_fat_dma_alloc.c
|
||||
tasks.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
console_buffer_usr.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
|
||||
@@ -45,6 +46,7 @@ target_link_libraries(px4_layer
|
||||
|
||||
add_library(px4_kernel_layer
|
||||
${KERNEL_SRCS}
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
|
||||
@@ -40,6 +40,7 @@ set(EXTRA_DEPENDS)
|
||||
add_library(px4_layer
|
||||
px4_posix_impl.cpp
|
||||
tasks.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
px4_sem.cpp
|
||||
px4_init.cpp
|
||||
lib_crc32.c
|
||||
|
||||
@@ -35,6 +35,7 @@
|
||||
set(QURT_LAYER_SRCS
|
||||
drv_hrt.cpp
|
||||
tasks.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
px4_qurt_impl.cpp
|
||||
main.cpp
|
||||
qurt_log.cpp
|
||||
|
||||
@@ -43,220 +43,6 @@
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
/**
|
||||
* @class ModuleBase
|
||||
* Base class for modules, implementing common functionality,
|
||||
* such as 'start', 'stop' and 'status' commands.
|
||||
* Currently does not support modules which allow multiple instances,
|
||||
* such as mavlink.
|
||||
*
|
||||
* The class is implemented as curiously recurring template pattern (CRTP).
|
||||
* It allows to have a static object in the base class that is different for
|
||||
* each module type, and call static methods from the base class.
|
||||
*
|
||||
* @note Required methods for a derived class:
|
||||
* When running in its own thread:
|
||||
* static int task_spawn(int argc, char *argv[])
|
||||
* {
|
||||
* // call px4_task_spawn_cmd() with &run_trampoline as startup method
|
||||
* // optional: wait until _object is not null, which means the task got initialized (use a timeout)
|
||||
* // set _task_id and return 0
|
||||
* // on error return != 0 (and _task_id must be -1)
|
||||
* }
|
||||
*
|
||||
* static T *instantiate(int argc, char *argv[])
|
||||
* {
|
||||
* // this is called from within the new thread, from run_trampoline()
|
||||
* // parse the arguments
|
||||
* // create a new object T & return it
|
||||
* // or return nullptr on error
|
||||
* }
|
||||
*
|
||||
* static int custom_command(int argc, char *argv[])
|
||||
* {
|
||||
* // support for custom commands
|
||||
* // if none are supported, just do:
|
||||
* return print_usage("unrecognized command");
|
||||
* }
|
||||
*
|
||||
* static int print_usage(const char *reason = nullptr)
|
||||
* {
|
||||
* // use the PRINT_MODULE_* methods...
|
||||
* }
|
||||
*
|
||||
* When running on the work queue:
|
||||
* - custom_command & print_usage as above
|
||||
* static int task_spawn(int argc, char *argv[]) {
|
||||
* // parse the arguments
|
||||
* // set _object (here or from the work_queue() callback)
|
||||
* // call work_queue() (with a custom cycle trampoline)
|
||||
* // optional: wait until _object is not null, which means the task got initialized (use a timeout)
|
||||
* // set _task_id to task_id_is_work_queue and return 0
|
||||
* // on error return != 0 (and _task_id must be -1)
|
||||
* }
|
||||
*/
|
||||
template<class T>
|
||||
class ModuleBase : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ModuleBase() : Node(MODULE_NAME)
|
||||
{
|
||||
|
||||
RCLCPP_INFO(get_logger(), "constructing %lu", hrt_absolute_time());
|
||||
}
|
||||
|
||||
virtual ~ModuleBase() {}
|
||||
|
||||
/**
|
||||
* @brief main Main entry point to the module that should be
|
||||
* called directly from the module's main method.
|
||||
* @param argc The task argument count.
|
||||
* @param argc Pointer to the task argument variable array.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
*/
|
||||
static int main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Print the status if the module is running. This can be overridden by the module to provide
|
||||
* more specific information.
|
||||
* @return Returns 0 iff successful, -1 otherwise.
|
||||
*/
|
||||
virtual int print_status()
|
||||
{
|
||||
PX4_INFO("running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main loop method for modules running in their own thread. Called from run_trampoline().
|
||||
* This method must return when should_exit() returns true.
|
||||
*/
|
||||
virtual void run()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the status of the module.
|
||||
* @return Returns true if the module is running, false otherwise.
|
||||
*/
|
||||
static bool is_running()
|
||||
{
|
||||
return _task_id != -1;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Tells the module to stop (used from outside or inside the module thread).
|
||||
*/
|
||||
virtual void request_stop()
|
||||
{
|
||||
_task_should_exit.store(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if the module should stop (used within the module thread).
|
||||
* @return Returns True iff successful, false otherwise.
|
||||
*/
|
||||
bool should_exit() const
|
||||
{
|
||||
return _task_should_exit.load();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Exits the module and delete the object. Called from within the module's thread.
|
||||
* For work queue modules, this needs to be called from the derived class in the
|
||||
* cycle method, when should_exit() returns true.
|
||||
*/
|
||||
static void exit_and_cleanup()
|
||||
{
|
||||
// Take the lock here:
|
||||
// - if startup fails and we're faster than the parent thread, it will set
|
||||
// _task_id and subsequently it will look like the task is running.
|
||||
// - deleting the object must take place inside the lock.
|
||||
lock_module();
|
||||
|
||||
delete _object.load();
|
||||
_object.store(nullptr);
|
||||
|
||||
_task_id = -1; // Signal a potentially waiting thread for the module to exit that it can continue.
|
||||
unlock_module();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn().
|
||||
* @return Returns 0 iff successful, -1 on timeout or otherwise.
|
||||
*/
|
||||
static int wait_until_running(int timeout_ms = 1000)
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
do {
|
||||
px4_usleep(2000);
|
||||
|
||||
} while (!_object.load() && ++i < timeout_ms / 2);
|
||||
|
||||
if (i >= timeout_ms / 2) {
|
||||
PX4_ERR("Timed out while waiting for thread to start");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the module's object instance, (this is null if it's not running).
|
||||
*/
|
||||
static T *get_instance()
|
||||
{
|
||||
return (T *)_object.load();
|
||||
}
|
||||
|
||||
/**
|
||||
* @var _object Instance if the module is running.
|
||||
* @note There will be one instance for each template type.
|
||||
*/
|
||||
static px4::atomic<T *> _object;
|
||||
|
||||
/** @var _task_id The task handle: -1 = invalid, otherwise task is assumed to be running. */
|
||||
static int _task_id;
|
||||
|
||||
/** @var task_id_is_work_queue Value to indicate if the task runs on the work queue. */
|
||||
static constexpr const int task_id_is_work_queue = -2;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief lock_module Mutex to lock the module thread.
|
||||
*/
|
||||
static void lock_module()
|
||||
{
|
||||
pthread_mutex_lock(&px4_modules_mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief unlock_module Mutex to unlock the module thread.
|
||||
*/
|
||||
static void unlock_module()
|
||||
{
|
||||
pthread_mutex_unlock(&px4_modules_mutex);
|
||||
}
|
||||
|
||||
/** @var _task_should_exit Boolean flag to indicate if the task should exit. */
|
||||
px4::atomic_bool _task_should_exit{false};
|
||||
};
|
||||
|
||||
template<class T>
|
||||
px4::atomic<T *> ModuleBase<T>::_object{nullptr};
|
||||
|
||||
template<class T>
|
||||
int ModuleBase<T>::_task_id = -1;
|
||||
|
||||
|
||||
#endif /* __cplusplus */
|
||||
|
||||
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
add_library(px4_layer STATIC
|
||||
drv_hrt.cpp
|
||||
px4_sem.cpp
|
||||
${PX4_SOURCE_DIR}/platforms/common/module_base.cpp
|
||||
)
|
||||
target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4")
|
||||
target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable
|
||||
|
||||
@@ -37,6 +37,8 @@
|
||||
|
||||
#include "voxl_esc.hpp"
|
||||
|
||||
ModuleBase::Descriptor VoxlEsc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
// future use:
|
||||
#define MODALAI_PUBLISH_ESC_STATUS 0
|
||||
|
||||
@@ -425,8 +427,8 @@ int VoxlEsc::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -439,8 +441,8 @@ int VoxlEsc::task_spawn(int argc, char *argv[])
|
||||
// This will cause a crash on SLPI DSP
|
||||
// delete instance;
|
||||
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -693,12 +695,12 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
|
||||
/* start the driver if not running */
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
return VoxlEsc::task_spawn(argc, argv);
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("Not running");
|
||||
return -1;
|
||||
|
||||
@@ -761,7 +763,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
PX4_ERR("Reset ESC: %i", esc_id);
|
||||
cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
|
||||
cmd.response = false;
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -774,7 +776,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
|
||||
cmd.response = true;
|
||||
cmd.resp_delay_us = 2000;
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -787,7 +789,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
|
||||
cmd.response = true;
|
||||
cmd.resp_delay_us = 5000;
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -799,7 +801,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
PX4_ERR("Request tone for ESC mask: %i", esc_id);
|
||||
cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
|
||||
cmd.response = false;
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -808,14 +810,14 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
|
||||
} else if (!strcmp(verb, "led")) {
|
||||
if (led_mask <= 0x0FFF) {
|
||||
get_instance()->_led_rsc.test = true;
|
||||
get_instance()->_led_rsc.breath_en = false;
|
||||
get_instance<VoxlEsc>(desc)->_led_rsc.test = true;
|
||||
get_instance<VoxlEsc>(desc)->_led_rsc.breath_en = false;
|
||||
PX4_ERR("Request LED control for ESCs with mask: %i", led_mask);
|
||||
|
||||
get_instance()->_esc_chans[0].led = (led_mask & 0x0007);
|
||||
get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3;
|
||||
get_instance()->_esc_chans[2].led = (led_mask & 0x01C0) >> 6;
|
||||
get_instance()->_esc_chans[3].led = (led_mask & 0x0E00) >> 9;
|
||||
get_instance<VoxlEsc>(desc)->_esc_chans[0].led = (led_mask & 0x0007);
|
||||
get_instance<VoxlEsc>(desc)->_esc_chans[1].led = (led_mask & 0x0038) >> 3;
|
||||
get_instance<VoxlEsc>(desc)->_esc_chans[2].led = (led_mask & 0x01C0) >> 6;
|
||||
get_instance<VoxlEsc>(desc)->_esc_chans[3].led = (led_mask & 0x0E00) >> 9;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
@@ -851,7 +853,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
id_fb,
|
||||
cmd.buf,
|
||||
sizeof(cmd.buf),
|
||||
get_instance()->_extended_rpm);
|
||||
get_instance<VoxlEsc>(desc)->_extended_rpm);
|
||||
|
||||
cmd.response = true;
|
||||
cmd.repeats = repeat_count;
|
||||
@@ -862,7 +864,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
PX4_ERR("Feedback id debug: %i", id_fb);
|
||||
PX4_ERR("Sending UART ESC RPM command %i", rate);
|
||||
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -907,7 +909,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
|
||||
PX4_ERR("Feedback id debug: %i", id_fb);
|
||||
PX4_ERR("Sending UART ESC power command %i", rate);
|
||||
|
||||
return get_instance()->send_cmd_thread_safe(&cmd);
|
||||
return get_instance<VoxlEsc>(desc)->send_cmd_thread_safe(&cmd);
|
||||
|
||||
} else {
|
||||
print_usage("Invalid ESC ID, use 0-3");
|
||||
@@ -1422,7 +1424,7 @@ void VoxlEsc::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1446,7 +1448,7 @@ void VoxlEsc::Run()
|
||||
PX4_ERR("Failed to initialize device, exiting the module");
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1767,5 +1769,5 @@ extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]);
|
||||
|
||||
int voxl_esc_main(int argc, char *argv[])
|
||||
{
|
||||
return VoxlEsc::main(argc, argv);
|
||||
return ModuleBase::main(VoxlEsc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -58,9 +58,11 @@
|
||||
|
||||
using namespace device;
|
||||
|
||||
class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
|
||||
class VoxlEsc : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
VoxlEsc();
|
||||
virtual ~VoxlEsc();
|
||||
|
||||
|
||||
@@ -39,6 +39,8 @@
|
||||
#include <nuttx/ioexpander/gpio.h>
|
||||
#endif
|
||||
|
||||
ModuleBase::Descriptor ADC::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_publish_adc_report(publish_adc_report),
|
||||
@@ -362,8 +364,8 @@ int ADC::custom_command(int argc, char *argv[])
|
||||
const char *verb = argv[0];
|
||||
|
||||
if (!strcmp(verb, "test")) {
|
||||
if (is_running()) {
|
||||
return _object.load()->test();
|
||||
if (is_running(desc)) {
|
||||
return get_instance<ADC>(desc)->test();
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
@@ -378,8 +380,8 @@ int ADC::task_spawn(int argc, char *argv[])
|
||||
ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -390,8 +392,8 @@ int ADC::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -421,5 +423,5 @@ ADC driver.
|
||||
|
||||
extern "C" __EXPORT int board_adc_main(int argc, char *argv[])
|
||||
{
|
||||
return ADC::main(argc, argv);
|
||||
return ModuleBase::main(ADC::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -62,9 +62,11 @@ using namespace time_literals;
|
||||
|
||||
#define ADC_TOTAL_CHANNELS 32
|
||||
|
||||
class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
|
||||
class ADC : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS, bool publish_adc_report = true);
|
||||
|
||||
~ADC() override;
|
||||
|
||||
@@ -41,6 +41,8 @@
|
||||
#include <builtin/builtin.h>
|
||||
#include <sys/wait.h>
|
||||
|
||||
ModuleBase::Descriptor AuterionAutostarter::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
AuterionAutostarter::AuterionAutostarter() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
@@ -57,7 +59,7 @@ bool AuterionAutostarter::init()
|
||||
void AuterionAutostarter::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -65,7 +67,7 @@ void AuterionAutostarter::Run()
|
||||
_actuator_armed_sub.copy(&actuator_armed);
|
||||
|
||||
if (actuator_armed.armed) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -510,8 +512,8 @@ int AuterionAutostarter::task_spawn(int argc, char *argv[])
|
||||
AuterionAutostarter *instance = new AuterionAutostarter();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -522,8 +524,8 @@ int AuterionAutostarter::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -555,5 +557,5 @@ Driver for starting and auto-detecting different power monitors.
|
||||
|
||||
extern "C" __EXPORT int auterion_autostarter_main(int argc, char *argv[])
|
||||
{
|
||||
return AuterionAutostarter::main(argc, argv);
|
||||
return ModuleBase::main(AuterionAutostarter::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -71,10 +71,12 @@ private:
|
||||
struct i2c_master_s *_i2c {nullptr};
|
||||
};
|
||||
|
||||
class AuterionAutostarter : public ModuleBase<AuterionAutostarter>, public px4::ScheduledWorkItem
|
||||
class AuterionAutostarter : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
AuterionAutostarter();
|
||||
virtual ~AuterionAutostarter();
|
||||
|
||||
|
||||
@@ -45,6 +45,8 @@ __END_DECLS
|
||||
|
||||
#include <px4_platform_common/shutdown.h>
|
||||
|
||||
ModuleBase::Descriptor CdcAcmAutostart::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
#define USB_DEVICE_PATH "/dev/ttyACM0"
|
||||
|
||||
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
|
||||
@@ -95,7 +97,7 @@ int CdcAcmAutostart::Start()
|
||||
void CdcAcmAutostart::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -566,8 +568,8 @@ int CdcAcmAutostart::task_spawn(int argc, char *argv[])
|
||||
return ret;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -660,7 +662,7 @@ and continue to check for VBUS and start mavlink once it is detected.
|
||||
extern "C" __EXPORT int cdcacm_autostart_main(int argc, char *argv[])
|
||||
{
|
||||
#if defined(CONFIG_SYSTEM_CDCACM)
|
||||
return CdcAcmAutostart::main(argc, argv);
|
||||
return ModuleBase::main(CdcAcmAutostart::desc, argc, argv);
|
||||
#endif
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -44,9 +44,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class CdcAcmAutostart : public ModuleBase<CdcAcmAutostart>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class CdcAcmAutostart : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
CdcAcmAutostart();
|
||||
~CdcAcmAutostart() override;
|
||||
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
|
||||
#include "pga460.h"
|
||||
|
||||
ModuleBase::Descriptor PGA460::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
|
||||
PGA460::PGA460(const char *port)
|
||||
{
|
||||
@@ -751,6 +753,13 @@ int PGA460::take_measurement(const uint8_t mode)
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int PGA460::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return PGA460::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int PGA460::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
px4_main_t entry_point = (px4_main_t)&run_trampoline;
|
||||
@@ -765,7 +774,7 @@ int PGA460::task_spawn(int argc, char *argv[])
|
||||
return -errno;
|
||||
}
|
||||
|
||||
_task_id = task_id;
|
||||
desc.task_id = task_id;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -905,5 +914,5 @@ int PGA460::write_register(const uint8_t reg, const uint8_t val)
|
||||
|
||||
extern "C" __EXPORT int pga460_main(int argc, char *argv[])
|
||||
{
|
||||
return PGA460::main(argc, argv);
|
||||
return ModuleBase::main(PGA460::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -207,10 +207,12 @@
|
||||
#define P2_THR_15 0x0 //reg addr 0x7E
|
||||
#define THR_CRC 0x1D //reg addr 0x7F
|
||||
|
||||
class PGA460 : public ModuleBase<PGA460>
|
||||
class PGA460 : public ModuleBase
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
PGA460(const char *port = PGA460_DEFAULT_PORT);
|
||||
|
||||
virtual ~PGA460();
|
||||
@@ -245,6 +247,11 @@ public:
|
||||
*/
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @see ModuleBase
|
||||
*/
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Closes the serial port.
|
||||
* @return Returns 0 if success or ERRNO.
|
||||
|
||||
@@ -67,9 +67,11 @@ static constexpr uint32_t HXSRX0X_CONVERSION_INTERVAL{50_ms};
|
||||
// Maximum time to wait for a conversion to complete.
|
||||
static constexpr uint32_t HXSRX0X_CONVERSION_TIMEOUT{30_ms};
|
||||
|
||||
class SRF05 : public ModuleBase<SRF05>, public px4::ScheduledWorkItem
|
||||
class SRF05 : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
SRF05(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
virtual ~SRF05() override;
|
||||
|
||||
|
||||
@@ -46,6 +46,8 @@
|
||||
|
||||
#include <px4_arch/micro_hal.h>
|
||||
|
||||
ModuleBase::Descriptor SRF05::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
SRF05::SRF05(const uint8_t rotation) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_px4_rangefinder(0 /* no device type for GPIO input */, rotation)
|
||||
@@ -114,7 +116,7 @@ SRF05::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -195,8 +197,8 @@ int SRF05::task_spawn(int argc, char *argv[])
|
||||
SRF05 *instance = new SRF05(rotation);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -207,8 +209,8 @@ int SRF05::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -251,7 +253,7 @@ SRF05::print_status()
|
||||
|
||||
extern "C" __EXPORT int srf05_main(int argc, char *argv[])
|
||||
{
|
||||
return SRF05::main(argc, argv);
|
||||
return ModuleBase::main(SRF05::desc, argc, argv);
|
||||
}
|
||||
#else
|
||||
# error ("GPIO_ULTRASOUND_xxx not defined. Driver not supported.");
|
||||
|
||||
@@ -37,6 +37,8 @@
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
ModuleBase::Descriptor DShot::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
char DShot::_telemetry_device[] {};
|
||||
bool DShot::_telemetry_swap_rxtx{false};
|
||||
px4::atomic_bool DShot::_request_telemetry_init{false};
|
||||
@@ -81,8 +83,8 @@ int DShot::task_spawn(int argc, char *argv[])
|
||||
DShot *instance = new DShot();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -93,8 +95,8 @@ int DShot::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -466,7 +468,7 @@ void DShot::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -698,16 +700,16 @@ int DShot::custom_command(int argc, char *argv[])
|
||||
|
||||
for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) {
|
||||
if (!strcmp(verb, commands[i].name)) {
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_ERR("module not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return get_instance()->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index);
|
||||
return get_instance<DShot>(desc)->send_command_thread_safe(commands[i].command, commands[i].num_repetitions, motor_index);
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = DShot::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
@@ -805,5 +807,5 @@ After saving, the reversed direction will be regarded as the normal one. So to r
|
||||
|
||||
extern "C" __EXPORT int dshot_main(int argc, char *argv[])
|
||||
{
|
||||
return DShot::main(argc, argv);
|
||||
return ModuleBase::main(DShot::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -57,9 +57,11 @@ static constexpr int DSHOT_DISARM_VALUE = 0;
|
||||
static constexpr int DSHOT_MIN_THROTTLE = 1;
|
||||
static constexpr int DSHOT_MAX_THROTTLE = 1999;
|
||||
|
||||
class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
|
||||
class DShot final : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
DShot();
|
||||
~DShot() override;
|
||||
|
||||
|
||||
@@ -67,6 +67,8 @@ using namespace device;
|
||||
namespace septentrio
|
||||
{
|
||||
|
||||
ModuleBase::Descriptor SeptentrioDriver::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
/**
|
||||
* RTC drift time when time synchronization is needed (in seconds).
|
||||
*/
|
||||
@@ -347,6 +349,13 @@ void SeptentrioDriver::run()
|
||||
|
||||
}
|
||||
|
||||
int SeptentrioDriver::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return SeptentrioDriver::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int SeptentrioDriver::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
return task_spawn(argc, argv, Instance::Main);
|
||||
@@ -372,14 +381,14 @@ int SeptentrioDriver::task_spawn(int argc, char *argv[], Instance instance)
|
||||
(char *const *)argv);
|
||||
|
||||
if (task_id < 0) {
|
||||
// `_task_id` of module that hasn't been started before or has been stopped should already be -1.
|
||||
// `desc.task_id` of module that hasn't been started before or has been stopped should already be -1.
|
||||
// This is just to make sure.
|
||||
_task_id = -1;
|
||||
desc.task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (instance == Instance::Main) {
|
||||
_task_id = task_id;
|
||||
desc.task_id = task_id;
|
||||
}
|
||||
|
||||
return 0;
|
||||
@@ -489,12 +498,12 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
|
||||
const char *failure_reason {"unknown command"};
|
||||
SeptentrioDriver *driver_instance;
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
driver_instance = get_instance();
|
||||
driver_instance = get_instance<SeptentrioDriver>(desc);
|
||||
|
||||
if (argc >= 1 && strcmp(argv[0], "reset") == 0) {
|
||||
if (argc == 2) {
|
||||
@@ -1861,5 +1870,5 @@ uint32_t SeptentrioDriver::get_parameter(const char *name, float *value)
|
||||
|
||||
extern "C" __EXPORT int septentrio_main(int argc, char *argv[])
|
||||
{
|
||||
return septentrio::SeptentrioDriver::main(argc, argv);
|
||||
return ModuleBase::main(septentrio::SeptentrioDriver::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -227,9 +227,11 @@ enum class ReceiverOutputTracker {
|
||||
PositioningMessages = DOP + PVTGeodetic + VelCovGeodetic + AttEuler + AttCovEuler,
|
||||
};
|
||||
|
||||
class SeptentrioDriver : public ModuleBase<SeptentrioDriver>, public device::Device
|
||||
class SeptentrioDriver : public ModuleBase, public device::Device
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate);
|
||||
~SeptentrioDriver() override;
|
||||
|
||||
@@ -244,6 +246,9 @@ public:
|
||||
|
||||
static int task_spawn(int argc, char *argv[], Instance instance);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Secondary run trampoline to support two driver instances.
|
||||
*/
|
||||
|
||||
+21
-6
@@ -117,7 +117,7 @@ struct GPS_Sat_Info {
|
||||
static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2040);
|
||||
|
||||
|
||||
class GPS : public ModuleBase<GPS>, public device::Device
|
||||
class GPS : public ModuleBase, public device::Device
|
||||
{
|
||||
public:
|
||||
|
||||
@@ -133,6 +133,8 @@ public:
|
||||
unsigned configured_baudrate);
|
||||
~GPS() override;
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
@@ -150,6 +152,11 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/**
|
||||
* task spawn trampoline for the primary GPS
|
||||
*/
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* task spawn trampoline for the secondary GPS
|
||||
*/
|
||||
@@ -302,6 +309,7 @@ private:
|
||||
|
||||
px4::atomic_bool GPS::_is_gps_main_advertised{false};
|
||||
px4::atomic<GPS *> GPS::_secondary_instance{nullptr};
|
||||
ModuleBase::Descriptor GPS::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
@@ -1418,12 +1426,12 @@ int
|
||||
GPS::custom_command(int argc, char *argv[])
|
||||
{
|
||||
// Check if the driver is running.
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("not running");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
GPS *_instance = get_instance();
|
||||
GPS *_instance = get_instance<GPS>(desc);
|
||||
|
||||
bool res = false;
|
||||
|
||||
@@ -1517,17 +1525,24 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
|
||||
entry_point, (char *const *)argv);
|
||||
|
||||
if (task_id < 0) {
|
||||
_task_id = -1;
|
||||
desc.task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (instance == Instance::Main) {
|
||||
_task_id = task_id;
|
||||
desc.task_id = task_id;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int GPS::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return GPS::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int GPS::run_trampoline_secondary(int argc, char *argv[])
|
||||
{
|
||||
// the task name is the first argument
|
||||
@@ -1693,5 +1708,5 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
|
||||
int
|
||||
gps_main(int argc, char *argv[])
|
||||
{
|
||||
return GPS::main(argc, argv);
|
||||
return ModuleBase::main(GPS::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -48,6 +48,8 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_io_heater.h>
|
||||
|
||||
ModuleBase::Descriptor Heater::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
#if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED)
|
||||
// Heater on some boards is on IO MCU
|
||||
// Use ioctl calls to IO driver to turn heater on/off
|
||||
@@ -75,7 +77,7 @@ Heater::~Heater()
|
||||
int Heater::custom_command(int argc, char *argv[])
|
||||
{
|
||||
// Check if the driver is running.
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("not running");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -180,7 +182,7 @@ void Heater::Run()
|
||||
}
|
||||
|
||||
#endif
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -292,8 +294,8 @@ int Heater::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(heater);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(heater);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
heater->start();
|
||||
return 0;
|
||||
@@ -334,5 +336,5 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE
|
||||
|
||||
extern "C" __EXPORT int heater_main(int argc, char *argv[])
|
||||
{
|
||||
return Heater::main(argc, argv);
|
||||
return ModuleBase::main(Heater::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -60,9 +60,11 @@ using namespace time_literals;
|
||||
#define CONTROLLER_PERIOD_DEFAULT 10000
|
||||
#define TEMPERATURE_TARGET_THRESHOLD 2.5f
|
||||
|
||||
class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
Heater();
|
||||
|
||||
virtual ~Heater();
|
||||
|
||||
@@ -43,6 +43,6 @@
|
||||
|
||||
extern "C" __EXPORT int eulernav_bahrs_main(int argc, char *argv[])
|
||||
{
|
||||
EulerNavDriver::main(argc, argv);
|
||||
ModuleBase::main(EulerNavDriver::desc, argc, argv);
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -52,20 +52,27 @@ EulerNavDriver::~EulerNavDriver()
|
||||
deinitialize();
|
||||
}
|
||||
|
||||
int EulerNavDriver::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return EulerNavDriver::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int EulerNavDriver::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
int task_id = px4_task_spawn_cmd("bahrs", SCHED_DEFAULT, SCHED_PRIORITY_FAST_DRIVER,
|
||||
Config::TASK_STACK_SIZE, (px4_main_t)&run_trampoline, argv);
|
||||
|
||||
if (task_id < 0) {
|
||||
_task_id = -1;
|
||||
desc.task_id = -1;
|
||||
PX4_ERR("Failed to spawn task.");
|
||||
|
||||
} else {
|
||||
_task_id = task_id;
|
||||
desc.task_id = task_id;
|
||||
}
|
||||
|
||||
return (_task_id < 0) ? 1 : 0;
|
||||
return (desc.task_id < 0) ? 1 : 0;
|
||||
}
|
||||
|
||||
EulerNavDriver *EulerNavDriver::instantiate(int argc, char *argv[])
|
||||
@@ -232,6 +239,8 @@ void EulerNavDriver::deinitialize()
|
||||
_is_initialized = false;
|
||||
}
|
||||
|
||||
ModuleBase::Descriptor EulerNavDriver::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
void EulerNavDriver::processDataBuffer()
|
||||
{
|
||||
static_assert(Config::MIN_MESSAGE_LENGTH >= (sizeof(CSerialProtocol::SMessageHeader) + sizeof(CSerialProtocol::CrcType_t)));
|
||||
|
||||
@@ -45,9 +45,11 @@
|
||||
#include <containers/Array.hpp>
|
||||
#include "CSerialProtocol.h"
|
||||
|
||||
class EulerNavDriver : public ModuleBase<EulerNavDriver>, public ModuleParams
|
||||
class EulerNavDriver : public ModuleBase, public ModuleParams
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
/// @brief Class constructor
|
||||
/// @param device_name Serial port to open
|
||||
EulerNavDriver(const char *device_name);
|
||||
@@ -57,6 +59,9 @@ public:
|
||||
/// @brief Required by ModuleBase
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/// @brief Required by ModuleBase
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/// @brief Required by ModuleBase
|
||||
static EulerNavDriver *instantiate(int argc, char *argv[]);
|
||||
|
||||
|
||||
@@ -44,6 +44,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor ILabs::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
// GPS epoch: 1980-01-06 00:00:00 UTC
|
||||
constexpr uint64_t GPS_EPOCH_SECS = 315964800ULL;
|
||||
|
||||
@@ -152,8 +154,8 @@ int ILabs::task_spawn(int argc, char *argv[]) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
@@ -227,7 +229,7 @@ int ILabs::print_status() {
|
||||
void ILabs::Run() {
|
||||
if (should_exit()) {
|
||||
_sensor.deinit();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -530,5 +532,5 @@ void ILabs::processData(InertialLabs::SensorsData *data) {
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int ilabs_main(int argc, char *argv[]) {
|
||||
return ILabs::main(argc, argv);
|
||||
return ModuleBase::main(ILabs::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -59,8 +59,10 @@
|
||||
|
||||
#include "sensor.h"
|
||||
|
||||
class ILabs : public ModuleBase<ILabs>, public ModuleParams, public px4::ScheduledWorkItem {
|
||||
class ILabs : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem {
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
ILabs(const char *port);
|
||||
~ILabs() override;
|
||||
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
#include "MicroStrain.hpp"
|
||||
|
||||
ModuleBase::Descriptor MicroStrain::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
device::Serial device_uart{};
|
||||
|
||||
MicroStrain::MicroStrain(const char *uart_port) :
|
||||
@@ -1851,7 +1853,7 @@ void MicroStrain::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1911,8 +1913,8 @@ int MicroStrain::task_spawn(int argc, char *argv[])
|
||||
|
||||
if (dev == nullptr || strlen(dev) == 0) {
|
||||
print_usage("no device specified");
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -1921,8 +1923,8 @@ int MicroStrain::task_spawn(int argc, char *argv[])
|
||||
MicroStrain *instance = new MicroStrain(dev);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -1933,8 +1935,8 @@ int MicroStrain::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -1995,5 +1997,5 @@ $ microstrain stop
|
||||
|
||||
extern "C" __EXPORT int microstrain_main(int argc, char *argv[])
|
||||
{
|
||||
return MicroStrain::main(argc, argv);
|
||||
return ModuleBase::main(MicroStrain::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -86,9 +86,11 @@ using matrix::Vector2f;
|
||||
|
||||
static constexpr float sq(float x) { return x * x; };
|
||||
|
||||
class MicroStrain : public ModuleBase<MicroStrain>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class MicroStrain : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
MicroStrain(const char *_device);
|
||||
~MicroStrain() override;
|
||||
|
||||
|
||||
@@ -72,6 +72,8 @@
|
||||
|
||||
using matrix::Vector2f;
|
||||
|
||||
ModuleBase::Descriptor SbgEcom::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
SbgEcom::SbgEcom(const char *device_name, uint32_t baudrate, const char *config_file, const char *config_string):
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device_name)),
|
||||
@@ -952,7 +954,7 @@ void SbgEcom::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1065,8 +1067,8 @@ int SbgEcom::task_spawn(int argc, char **argv)
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_task_id = task_id_is_work_queue;
|
||||
_object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
instance->ScheduleNow();
|
||||
return PX4_OK;
|
||||
|
||||
@@ -1121,5 +1123,5 @@ int SbgEcom::print_status()
|
||||
|
||||
extern "C" __EXPORT int sbgecom_main(int argc, char **argv)
|
||||
{
|
||||
return SbgEcom::main(argc, argv);
|
||||
return ModuleBase::main(SbgEcom::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -63,10 +63,12 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
|
||||
class SbgEcom : public ModuleBase<SbgEcom>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class SbgEcom : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
SbgEcom(const char *port, uint32_t baudrate, const char *config_file, const char *config_string);
|
||||
~SbgEcom() override;
|
||||
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
|
||||
using matrix::Vector2f;
|
||||
|
||||
ModuleBase::Descriptor VectorNav::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
VectorNav::VectorNav(const char *port) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
|
||||
@@ -682,7 +684,7 @@ void VectorNav::Run()
|
||||
if (should_exit()) {
|
||||
VnSensor_unregisterAsyncPacketReceivedHandler(&_vs);
|
||||
VnSensor_disconnect(&_vs);
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
|
||||
} else if (!_initialized) {
|
||||
@@ -797,8 +799,8 @@ int VectorNav::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
@@ -858,5 +860,5 @@ $ vectornav stop
|
||||
|
||||
extern "C" __EXPORT int vectornav_main(int argc, char *argv[])
|
||||
{
|
||||
return VectorNav::main(argc, argv);
|
||||
return ModuleBase::main(VectorNav::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -75,9 +75,11 @@ extern "C" {
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class VectorNav : public ModuleBase<VectorNav>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class VectorNav : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
VectorNav(const char *port);
|
||||
~VectorNav() override;
|
||||
|
||||
|
||||
@@ -49,9 +49,11 @@
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <drivers/drv_neopixel.h>
|
||||
|
||||
class NEOPIXEL : public px4::ScheduledWorkItem, public ModuleBase<NEOPIXEL>
|
||||
class NEOPIXEL : public px4::ScheduledWorkItem, public ModuleBase
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
NEOPIXEL(unsigned int number_of_packages);
|
||||
virtual ~NEOPIXEL();
|
||||
|
||||
@@ -84,6 +86,8 @@ private:
|
||||
neopixel::NeoLEDData *_leds;
|
||||
};
|
||||
|
||||
ModuleBase::Descriptor NEOPIXEL::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
NEOPIXEL::NEOPIXEL(unsigned int number_of_packages) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
|
||||
_number_of_packages(number_of_packages)
|
||||
@@ -131,8 +135,8 @@ int NEOPIXEL::task_spawn(int argc, char *argv[])
|
||||
NEOPIXEL *instance = new NEOPIXEL(number_of_packages);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -143,8 +147,8 @@ int NEOPIXEL::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
@@ -190,7 +194,7 @@ void NEOPIXEL::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -246,5 +250,5 @@ void NEOPIXEL::Run()
|
||||
|
||||
extern "C" __EXPORT int neopixel_main(int argc, char *argv[])
|
||||
{
|
||||
return NEOPIXEL::main(argc, argv);
|
||||
return ModuleBase::main(NEOPIXEL::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
|
||||
using namespace pwm_out;
|
||||
|
||||
ModuleBase::Descriptor LinuxPWMOut::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
LinuxPWMOut::LinuxPWMOut() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
|
||||
@@ -77,8 +79,8 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
|
||||
LinuxPWMOut *instance = new LinuxPWMOut();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -89,8 +91,8 @@ int LinuxPWMOut::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -108,7 +110,7 @@ void LinuxPWMOut::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -166,5 +168,5 @@ Linux PWM output driver with board-specific backend implementation.
|
||||
|
||||
extern "C" __EXPORT int linux_pwm_out_main(int argc, char *argv[])
|
||||
{
|
||||
return LinuxPWMOut::main(argc, argv);
|
||||
return ModuleBase::main(LinuxPWMOut::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -49,9 +49,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class LinuxPWMOut : public ModuleBase<LinuxPWMOut>, public OutputModuleInterface
|
||||
class LinuxPWMOut : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
LinuxPWMOut();
|
||||
virtual ~LinuxPWMOut();
|
||||
|
||||
|
||||
@@ -66,6 +66,8 @@
|
||||
|
||||
#include "MspV1.hpp"
|
||||
|
||||
ModuleBase::Descriptor MspOsd::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
//OSD elements positions
|
||||
//in betaflight configurator set OSD elements to your desired positions and in CLI type "set osd" to retreieve the numbers.
|
||||
//234 -> not visible. Horizontally 2048-2074(spacing 1), vertically 2048-2528(spacing 32). 26 characters X 15 lines
|
||||
@@ -246,7 +248,7 @@ void MspOsd::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -588,8 +590,8 @@ int MspOsd::task_spawn(int argc, char *argv[])
|
||||
MspOsd *instance = new MspOsd(device);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -600,8 +602,8 @@ int MspOsd::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -700,8 +702,8 @@ int MspOsd::custom_command(int argc, char *argv[])
|
||||
if (argc == 1) {
|
||||
PX4_INFO("Please provide a channel");
|
||||
|
||||
} else if (is_running() && _object.load()) {
|
||||
MspOsd *object = _object.load();
|
||||
} else if (is_running(desc) && desc.object.load()) {
|
||||
MspOsd *object = get_instance<MspOsd>(desc);
|
||||
int ret = object->set_channel(argv[1]);
|
||||
|
||||
if (ret == -1) {
|
||||
@@ -748,5 +750,5 @@ $ msp_osd
|
||||
|
||||
extern "C" __EXPORT int msp_osd_main(int argc, char *argv[])
|
||||
{
|
||||
return MspOsd::main(argc, argv);
|
||||
return ModuleBase::main(MspOsd::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -100,9 +100,11 @@ enum SymbolIndex : uint8_t {
|
||||
POWER = 21
|
||||
};
|
||||
|
||||
class MspOsd : public ModuleBase<MspOsd>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class MspOsd : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
MspOsd(const char *device);
|
||||
|
||||
~MspOsd() override;
|
||||
|
||||
@@ -57,9 +57,11 @@
|
||||
using namespace drv_pca9685_pwm;
|
||||
using namespace time_literals;
|
||||
|
||||
class PCA9685Wrapper : public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
|
||||
class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
PCA9685Wrapper();
|
||||
~PCA9685Wrapper() override;
|
||||
PCA9685Wrapper(const PCA9685Wrapper &) = delete;
|
||||
@@ -113,6 +115,8 @@ private:
|
||||
int registers_check();
|
||||
};
|
||||
|
||||
ModuleBase::Descriptor PCA9685Wrapper::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
PCA9685Wrapper::PCA9685Wrapper() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
|
||||
@@ -187,7 +191,7 @@ void PCA9685Wrapper::Run()
|
||||
delete pca9685;
|
||||
pca9685 = nullptr;
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -415,8 +419,8 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) {
|
||||
auto *instance = new PCA9685Wrapper();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->pca9685 = new PCA9685(iicbus, address);
|
||||
if(instance->pca9685==nullptr){
|
||||
@@ -438,8 +442,8 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) {
|
||||
|
||||
driverInstanceAllocFailed:
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -470,5 +474,5 @@ void PCA9685Wrapper::updateParams() {
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){
|
||||
return PCA9685Wrapper::main(argc, argv);
|
||||
return ModuleBase::main(PCA9685Wrapper::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -45,6 +45,8 @@
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
ModuleBase::Descriptor PPSCapture::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
PPSCapture::PPSCapture() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
@@ -109,7 +111,7 @@ bool PPSCapture::init()
|
||||
void PPSCapture::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -170,8 +172,8 @@ int PPSCapture::task_spawn(int argc, char *argv[])
|
||||
PPSCapture *instance = new PPSCapture();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -182,8 +184,8 @@ int PPSCapture::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -215,14 +217,14 @@ This implements capturing PPS information from the GNSS module and calculates th
|
||||
|
||||
void PPSCapture::stop()
|
||||
{
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int pps_capture_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running()) {
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running(PPSCapture::desc)) {
|
||||
PPSCapture::stop();
|
||||
}
|
||||
|
||||
return PPSCapture::main(argc, argv);
|
||||
return ModuleBase::main(PPSCapture::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -44,9 +44,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class PPSCapture : public ModuleBase<PPSCapture>, public px4::ScheduledWorkItem
|
||||
class PPSCapture : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
PPSCapture();
|
||||
virtual ~PPSCapture();
|
||||
|
||||
|
||||
@@ -34,6 +34,8 @@
|
||||
#include "pwm_input.h"
|
||||
#include <px4_arch/io_timer.h>
|
||||
|
||||
ModuleBase::Descriptor PWMIN::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
int
|
||||
PWMIN::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
@@ -44,8 +46,8 @@ PWMIN::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(pwmin);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(pwmin);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
pwmin->start();
|
||||
|
||||
@@ -148,7 +150,7 @@ PWMIN::pwmin_tim_isr(int irq, void *context, void *arg)
|
||||
/* ack the interrupts we just read */
|
||||
rSR = 0;
|
||||
|
||||
auto obj = get_instance();
|
||||
auto obj = get_instance<PWMIN>(desc);
|
||||
|
||||
if (obj != nullptr) {
|
||||
obj->publish(status, period, pulse_width);
|
||||
@@ -218,5 +220,5 @@ PWMIN::custom_command(int argc, char *argv[])
|
||||
|
||||
extern "C" __EXPORT int pwm_input_main(int argc, char *argv[])
|
||||
{
|
||||
return PWMIN::main(argc, argv);
|
||||
return ModuleBase::main(PWMIN::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -120,9 +120,11 @@
|
||||
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
|
||||
#endif
|
||||
|
||||
class PWMIN : public ModuleBase<PWMIN>
|
||||
class PWMIN : public ModuleBase
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
void start();
|
||||
void publish(uint16_t status, uint32_t period, uint32_t pulse_width);
|
||||
int print_status() override;
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
ModuleBase::Descriptor PWMOut::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
PWMOut::PWMOut() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
@@ -158,7 +160,7 @@ void PWMOut::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -202,8 +204,8 @@ int PWMOut::task_spawn(int argc, char *argv[])
|
||||
return -1;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return 0;
|
||||
}
|
||||
@@ -332,5 +334,5 @@ px4io driver is used for main ones.
|
||||
|
||||
extern "C" __EXPORT int pwm_out_main(int argc, char *argv[])
|
||||
{
|
||||
return PWMOut::main(argc, argv);
|
||||
return ModuleBase::main(PWMOut::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -53,9 +53,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class PWMOut final : public ModuleBase<PWMOut>, public OutputModuleInterface
|
||||
class PWMOut final : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
PWMOut();
|
||||
~PWMOut() override;
|
||||
|
||||
|
||||
+22
-17
@@ -99,9 +99,11 @@ using namespace time_literals;
|
||||
*
|
||||
* Encapsulates PX4FMU to PX4IO communications modeled as file operations.
|
||||
*/
|
||||
class PX4IO : public cdev::CDev, public ModuleBase<PX4IO>, public OutputModuleInterface
|
||||
class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
@@ -339,6 +341,8 @@ private:
|
||||
)
|
||||
};
|
||||
|
||||
ModuleBase::Descriptor PX4IO::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
#define PX4IO_DEVICE_PATH "/dev/px4io"
|
||||
|
||||
PX4IO::PX4IO(device::Device *interface) :
|
||||
@@ -512,7 +516,7 @@ void PX4IO::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1513,7 +1517,8 @@ int PX4IO::bind(int argc, char *argv[])
|
||||
pulses = atoi(argv[1]);
|
||||
}
|
||||
|
||||
get_instance()->ioctl(nullptr, DSM_BIND_START, pulses);
|
||||
get_instance<PX4IO>(desc)->ioctl(nullptr, DSM_BIND_START, pulses);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1529,8 +1534,8 @@ int PX4IO::task_spawn(int argc, char *argv[])
|
||||
PX4IO *instance = new PX4IO(interface);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
@@ -1541,8 +1546,8 @@ int PX4IO::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -1556,7 +1561,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "checkcrc")) {
|
||||
if (is_running()) {
|
||||
if (is_running(desc)) {
|
||||
PX4_ERR("io must be stopped");
|
||||
return 1;
|
||||
}
|
||||
@@ -1566,7 +1571,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(verb, "update")) {
|
||||
|
||||
if (is_running()) {
|
||||
if (is_running(desc)) {
|
||||
PX4_ERR("io must be stopped");
|
||||
return 1;
|
||||
}
|
||||
@@ -1656,7 +1661,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
|
||||
|
||||
/* commands below here require a started driver */
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_ERR("not running");
|
||||
return 1;
|
||||
}
|
||||
@@ -1668,7 +1673,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
uint8_t level = atoi(argv[1]);
|
||||
int ret = get_instance()->ioctl(nullptr, PX4IO_SET_DEBUG, level);
|
||||
int ret = get_instance<PX4IO>(desc)->ioctl(nullptr, PX4IO_SET_DEBUG, level);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("SET_DEBUG failed: %d", ret);
|
||||
@@ -1680,7 +1685,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "bind")) {
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_ERR("io must be running");
|
||||
return 1;
|
||||
}
|
||||
@@ -1689,7 +1694,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "sbus1_out")) {
|
||||
int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
|
||||
int ret = get_instance<PX4IO>(desc)->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("S.BUS v1 failed (%i)", ret);
|
||||
@@ -1700,7 +1705,7 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "sbus2_out")) {
|
||||
int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
|
||||
int ret = get_instance<PX4IO>(desc)->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("S.BUS v2 failed (%i)", ret);
|
||||
@@ -1711,12 +1716,12 @@ int PX4IO::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test_fmu_fail")) {
|
||||
get_instance()->test_fmu_fail(true);
|
||||
get_instance<PX4IO>(desc)->test_fmu_fail(true);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "test_fmu_ok")) {
|
||||
get_instance()->test_fmu_fail(false);
|
||||
get_instance<PX4IO>(desc)->test_fmu_fail(false);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1764,5 +1769,5 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[])
|
||||
PX4_INFO("PX4IO Not Supported");
|
||||
return -1;
|
||||
}
|
||||
return PX4IO::main(argc, argv);
|
||||
return ModuleBase::main(PX4IO::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -44,6 +44,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor CrsfRc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
#define CRSF_BAUDRATE 420000
|
||||
|
||||
CrsfRc::CrsfRc(const char *device) :
|
||||
@@ -109,8 +111,8 @@ int CrsfRc::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
@@ -128,7 +130,7 @@ void CrsfRc::Run()
|
||||
_uart = nullptr;
|
||||
}
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -548,7 +550,7 @@ int CrsfRc::custom_command(int argc, char *argv[])
|
||||
#ifdef CONFIG_RC_CRSF_INJECT
|
||||
|
||||
if (!strcmp(argv[0], "start")) {
|
||||
if (is_running()) {
|
||||
if (is_running(desc)) {
|
||||
return print_usage("already running");
|
||||
}
|
||||
|
||||
@@ -559,7 +561,7 @@ int CrsfRc::custom_command(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
return print_usage("not running");
|
||||
}
|
||||
|
||||
@@ -612,5 +614,5 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem
|
||||
|
||||
extern "C" __EXPORT int crsf_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return CrsfRc::main(argc, argv);
|
||||
return ModuleBase::main(CrsfRc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -56,9 +56,11 @@
|
||||
|
||||
using namespace device;
|
||||
|
||||
class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
CrsfRc(const char *device);
|
||||
~CrsfRc() override;
|
||||
|
||||
|
||||
@@ -39,6 +39,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor DsmRc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
DsmRc::DsmRc(const char *device) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
|
||||
@@ -106,8 +108,8 @@ int DsmRc::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleOnInterval(_current_update_interval);
|
||||
|
||||
@@ -136,7 +138,7 @@ void DsmRc::Run()
|
||||
|
||||
dsm_deinit();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -362,7 +364,7 @@ int DsmRc::custom_command(int argc, char *argv[])
|
||||
|
||||
#endif // SPEKTRUM_POWER
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = DsmRc::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
@@ -419,5 +421,5 @@ This module does Spektrum DSM RC input parsing.
|
||||
|
||||
extern "C" __EXPORT int dsm_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return DsmRc::main(argc, argv);
|
||||
return ModuleBase::main(DsmRc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -52,10 +52,12 @@
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class DsmRc : public ModuleBase<DsmRc>, public px4::ScheduledWorkItem
|
||||
class DsmRc : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
DsmRc(const char *device);
|
||||
virtual ~DsmRc();
|
||||
|
||||
|
||||
@@ -36,6 +36,8 @@
|
||||
#include <termios.h>
|
||||
#include <math.h>
|
||||
|
||||
ModuleBase::Descriptor GhstRc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
GhstRc::GhstRc(const char *device) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
|
||||
@@ -101,8 +103,8 @@ int GhstRc::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleOnInterval(_current_update_interval);
|
||||
|
||||
@@ -129,7 +131,7 @@ void GhstRc::Run()
|
||||
|
||||
close(_rcs_fd);
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -255,7 +257,7 @@ void GhstRc::Run()
|
||||
|
||||
int GhstRc::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = GhstRc::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
@@ -307,5 +309,5 @@ This module does Ghost (GHST) RC input parsing.
|
||||
|
||||
extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return GhstRc::main(argc, argv);
|
||||
return ModuleBase::main(GhstRc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -55,10 +55,12 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class GhstRc : public ModuleBase<GhstRc>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
GhstRc(const char *device);
|
||||
virtual ~GhstRc();
|
||||
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
#include <termios.h>
|
||||
|
||||
ModuleBase::Descriptor SbusRc::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
SbusRc::SbusRc(const char *device) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
|
||||
@@ -98,8 +100,8 @@ int SbusRc::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleOnInterval(_current_update_interval);
|
||||
|
||||
@@ -126,7 +128,7 @@ void SbusRc::Run()
|
||||
|
||||
close(_rcs_fd);
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -265,7 +267,7 @@ void SbusRc::Run()
|
||||
|
||||
int SbusRc::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = SbusRc::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
@@ -317,5 +319,5 @@ This module does SBUS RC input parsing.
|
||||
|
||||
extern "C" __EXPORT int sbus_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return SbusRc::main(argc, argv);
|
||||
return ModuleBase::main(SbusRc::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -53,10 +53,12 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class SbusRc : public ModuleBase<SbusRc>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class SbusRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
SbusRc(const char *device);
|
||||
virtual ~SbusRc();
|
||||
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor RCInput::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
constexpr char const *RCInput::RC_SCAN_STRING[];
|
||||
|
||||
RCInput::RCInput(const char *device) :
|
||||
@@ -181,8 +183,8 @@ RCInput::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleOnInterval(_current_update_interval);
|
||||
|
||||
@@ -338,7 +340,7 @@ void RCInput::swap_rx_tx()
|
||||
void RCInput::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -348,7 +350,7 @@ void RCInput::Run()
|
||||
|
||||
} else {
|
||||
PX4_ERR("init failed");
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -919,7 +921,7 @@ int RCInput::custom_command(int argc, char *argv[])
|
||||
#endif /* SPEKTRUM_POWER */
|
||||
|
||||
/* start the FMU if not running */
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = RCInput::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
@@ -1033,5 +1035,5 @@ This module does the RC input parsing and auto-selecting the method. Supported m
|
||||
|
||||
extern "C" __EXPORT int rc_input_main(int argc, char *argv[])
|
||||
{
|
||||
return RCInput::main(argc, argv);
|
||||
return ModuleBase::main(RCInput::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -67,10 +67,12 @@
|
||||
# include <systemlib/ppm_decode.h>
|
||||
#endif
|
||||
|
||||
class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class RCInput : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
RCInput(const char *device);
|
||||
virtual ~RCInput();
|
||||
|
||||
|
||||
@@ -44,6 +44,8 @@
|
||||
#include "Roboclaw.hpp"
|
||||
#include <termios.h>
|
||||
|
||||
ModuleBase::Descriptor Roboclaw::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
Roboclaw::Roboclaw(const char *device_name, const char *bad_rate_parameter) :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
@@ -170,7 +172,7 @@ void Roboclaw::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
_mixing_output.unregister();
|
||||
return;
|
||||
}
|
||||
@@ -476,8 +478,8 @@ int Roboclaw::task_spawn(int argc, char *argv[])
|
||||
Roboclaw *instance = new Roboclaw(device_name, baud_rate_parameter_value);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return OK;
|
||||
|
||||
@@ -486,8 +488,8 @@ int Roboclaw::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
printf("Ending task_spawn");
|
||||
|
||||
@@ -535,5 +537,5 @@ int Roboclaw::print_status()
|
||||
|
||||
extern "C" __EXPORT int roboclaw_main(int argc, char *argv[])
|
||||
{
|
||||
return Roboclaw::main(argc, argv);
|
||||
return ModuleBase::main(Roboclaw::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -54,9 +54,11 @@
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/wheel_encoders.h>
|
||||
|
||||
class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface
|
||||
class Roboclaw : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
/**
|
||||
* @param device_name Name of the serial port e.g. "/dev/ttyS2"
|
||||
* @param bad_rate_parameter Name of the parameter that holds the baud rate of this serial port
|
||||
|
||||
@@ -39,6 +39,8 @@
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
ModuleBase::Descriptor RPMCapture::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
RPMCapture::RPMCapture() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
ModuleParams(nullptr)
|
||||
@@ -98,7 +100,7 @@ bool RPMCapture::init()
|
||||
void RPMCapture::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -169,8 +171,8 @@ int RPMCapture::task_spawn(int argc, char *argv[])
|
||||
RPMCapture *instance = new RPMCapture();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -181,8 +183,8 @@ int RPMCapture::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -207,14 +209,14 @@ int RPMCapture::print_usage(const char *reason)
|
||||
|
||||
void RPMCapture::stop()
|
||||
{
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) {
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running(RPMCapture::desc)) {
|
||||
RPMCapture::stop();
|
||||
}
|
||||
|
||||
return RPMCapture::main(argc, argv);
|
||||
return ModuleBase::main(RPMCapture::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -47,9 +47,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class RPMCapture : public ModuleBase<RPMCapture>, public px4::ScheduledWorkItem, public ModuleParams
|
||||
class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, public ModuleParams
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
RPMCapture();
|
||||
virtual ~RPMCapture();
|
||||
|
||||
|
||||
@@ -40,6 +40,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor SafetyButton::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
static constexpr uint8_t CYCLE_COUNT{30}; /* safety switch must be held for 1 second to activate */
|
||||
|
||||
// Define the various LED flash sequences for each system state.
|
||||
@@ -150,7 +152,7 @@ void
|
||||
SafetyButton::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -183,8 +185,8 @@ SafetyButton::task_spawn(int argc, char *argv[])
|
||||
return ret;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -230,5 +232,5 @@ extern "C" __EXPORT int safety_button_main(int argc, char *argv[]);
|
||||
int
|
||||
safety_button_main(int argc, char *argv[])
|
||||
{
|
||||
return SafetyButton::main(argc, argv);
|
||||
return ModuleBase::main(SafetyButton::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -40,9 +40,11 @@
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <button/ButtonPublisher.hpp>
|
||||
|
||||
class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
|
||||
class SafetyButton : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
SafetyButton();
|
||||
~SafetyButton() override;
|
||||
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
ModuleBase::Descriptor TAP_ESC::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
|
||||
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)),
|
||||
_mixing_output{"TAP_ESC", channels_count, *this, MixingOutput::SchedulingPolicy::Auto, true},
|
||||
@@ -333,7 +335,7 @@ void TAP_ESC::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -356,7 +358,7 @@ void TAP_ESC::Run()
|
||||
|
||||
} else {
|
||||
PX4_ERR("init failed");
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -456,8 +458,8 @@ int TAP_ESC::task_spawn(int argc, char *argv[])
|
||||
return -1;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return 0;
|
||||
}
|
||||
@@ -510,5 +512,5 @@ tap_esc start -d /dev/ttyS2 -n <1-8>
|
||||
|
||||
extern "C" __EXPORT int tap_esc_main(int argc, char *argv[])
|
||||
{
|
||||
return TAP_ESC::main(argc, argv);
|
||||
return ModuleBase::main(TAP_ESC::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -79,9 +79,11 @@ using namespace time_literals;
|
||||
/*
|
||||
* This driver connects to TAP ESCs via serial.
|
||||
*/
|
||||
class TAP_ESC : public ModuleBase<TAP_ESC>, public OutputModuleInterface
|
||||
class TAP_ESC : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
TAP_ESC(const char *device, uint8_t channels_count);
|
||||
virtual ~TAP_ESC();
|
||||
|
||||
|
||||
@@ -46,6 +46,8 @@
|
||||
|
||||
#include "TattuCan.hpp"
|
||||
|
||||
ModuleBase::Descriptor TattuCan::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
extern orb_advert_t mavlink_log_pub;
|
||||
|
||||
TattuCan::TattuCan() :
|
||||
@@ -60,7 +62,7 @@ TattuCan::~TattuCan()
|
||||
void TattuCan::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -192,8 +194,8 @@ int TattuCan::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->start();
|
||||
return 0;
|
||||
@@ -221,7 +223,7 @@ Driver for reading data from the Tattu 12S 16000mAh smart battery.
|
||||
|
||||
int TattuCan::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("not running");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -231,5 +233,5 @@ int TattuCan::custom_command(int argc, char *argv[])
|
||||
|
||||
extern "C" __EXPORT int tattu_can_main(int argc, char *argv[])
|
||||
{
|
||||
return TattuCan::main(argc, argv);
|
||||
return ModuleBase::main(TattuCan::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -77,9 +77,11 @@ typedef struct {
|
||||
const void *payload;
|
||||
} CanFrame;
|
||||
|
||||
class TattuCan : public ModuleBase<TattuCan>, public px4::ScheduledWorkItem
|
||||
class TattuCan : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
TattuCan();
|
||||
|
||||
virtual ~TattuCan();
|
||||
|
||||
@@ -47,6 +47,8 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
ModuleBase::Descriptor IridiumSBD::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
static constexpr const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK", "SBD SESSION", "TEST"};
|
||||
|
||||
#define VERBOSE_INFO(...) if (_verbose) { PX4_INFO(__VA_ARGS__); }
|
||||
@@ -63,23 +65,30 @@ IridiumSBD::~IridiumSBD()
|
||||
deinit();
|
||||
}
|
||||
|
||||
int IridiumSBD::run_trampoline(int argc, char *argv[])
|
||||
{
|
||||
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
|
||||
return IridiumSBD::instantiate(ac, av);
|
||||
}, argc, argv);
|
||||
}
|
||||
|
||||
int IridiumSBD::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("iridiumsbd",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_SLOW_DRIVER,
|
||||
1350,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
desc.task_id = px4_task_spawn_cmd("iridiumsbd",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_SLOW_DRIVER,
|
||||
1350,
|
||||
(px4_main_t)&run_trampoline,
|
||||
(char *const *)argv);
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
if (desc.task_id < 0) {
|
||||
desc.task_id = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
// wait until task is up & running (max 6 seconds)
|
||||
if (wait_until_running(6000) < 0) {
|
||||
_task_id = -1;
|
||||
if (wait_until_running(desc, 6000) < 0) {
|
||||
desc.task_id = -1;
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -1080,13 +1089,13 @@ Creates a virtual serial port that another module can use for communication (e.g
|
||||
|
||||
int IridiumSBD::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
print_usage("not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "test")) {
|
||||
get_instance()->test(argc, argv);
|
||||
get_instance<IridiumSBD>(desc)->test(argc, argv);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1095,12 +1104,12 @@ int IridiumSBD::custom_command(int argc, char *argv[])
|
||||
|
||||
extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && IridiumSBD::is_running()) {
|
||||
if (argc >= 2 && !strcmp(argv[1], "stop") && IridiumSBD::is_running(IridiumSBD::desc)) {
|
||||
if (!IridiumSBD::can_stop()) {
|
||||
PX4_ERR("Device is used. Stop all users first (mavlink stop-all)");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
return IridiumSBD::main(argc, argv);
|
||||
return ModuleBase::main(IridiumSBD::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -99,15 +99,20 @@ typedef enum {
|
||||
* - Improve TX buffer handling:
|
||||
* - Do not reset the full TX buffer but delete the oldest HIGH_LATENCY2 message if one is in the buffer or delete the oldest message in general
|
||||
*/
|
||||
class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
|
||||
class IridiumSBD : public cdev::CDev, public ModuleBase
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
IridiumSBD();
|
||||
~IridiumSBD();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int run_trampoline(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static IridiumSBD *instantiate(int argc, char *argv[]);
|
||||
|
||||
@@ -136,7 +141,7 @@ public:
|
||||
*/
|
||||
int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
static bool can_stop() { return !get_instance()->_cdev_used.load(); }
|
||||
static bool can_stop() { return !get_instance<IridiumSBD>(desc)->_cdev_used.load(); }
|
||||
|
||||
private:
|
||||
int init(int argc, char *argv[]);
|
||||
|
||||
@@ -42,6 +42,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor ToneAlarm::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
ToneAlarm::ToneAlarm() :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
@@ -87,7 +89,7 @@ void ToneAlarm::Run()
|
||||
|
||||
if (should_exit()) {
|
||||
_tune_control_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -247,8 +249,8 @@ int ToneAlarm::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -275,5 +277,5 @@ This module is responsible for the tone alarm.
|
||||
|
||||
extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[])
|
||||
{
|
||||
return ToneAlarm::main(argc, argv);
|
||||
return ModuleBase::main(ToneAlarm::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -55,12 +55,14 @@
|
||||
|
||||
#include <string.h>
|
||||
|
||||
class ToneAlarm : public ModuleBase<ToneAlarm>, public px4::ScheduledWorkItem
|
||||
class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
ToneAlarm();
|
||||
~ToneAlarm() override;
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
|
||||
@@ -33,13 +33,15 @@
|
||||
|
||||
#include "SagetechMXS.hpp"
|
||||
|
||||
ModuleBase::Descriptor SagetechMXS::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
/***************************************
|
||||
* Workqueue Functions
|
||||
* *************************************/
|
||||
|
||||
extern "C" __EXPORT int sagetech_mxs_main(int argc, char *argv[])
|
||||
{
|
||||
return SagetechMXS::main(argc, argv);
|
||||
return ModuleBase::main(SagetechMXS::desc, argc, argv);
|
||||
}
|
||||
|
||||
SagetechMXS::SagetechMXS(const char *port) :
|
||||
@@ -90,8 +92,8 @@ int SagetechMXS::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -99,8 +101,8 @@ int SagetechMXS::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
@@ -137,7 +139,7 @@ int SagetechMXS::custom_command(int argc, char *argv[])
|
||||
{
|
||||
const char *verb = argv[0];
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
int ret = SagetechMXS::task_spawn(argc, argv);
|
||||
return ret;
|
||||
|
||||
@@ -152,12 +154,12 @@ int SagetechMXS::custom_command(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return get_instance()->handle_fid(fid);
|
||||
return get_instance<SagetechMXS>(desc)->handle_fid(fid);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "ident")) {
|
||||
get_instance()->_adsb_ident.set(1);
|
||||
return get_instance()->_adsb_ident.commit();
|
||||
get_instance<SagetechMXS>(desc)->_adsb_ident.set(1);
|
||||
return get_instance<SagetechMXS>(desc)->_adsb_ident.commit();
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "opmode")) {
|
||||
@@ -168,20 +170,20 @@ int SagetechMXS::custom_command(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
|
||||
} else if (!strcmp(opmode, "off") || !strcmp(opmode, "0")) {
|
||||
get_instance()->_mxs_op_mode.set(0);
|
||||
return get_instance()->_mxs_op_mode.commit();
|
||||
get_instance<SagetechMXS>(desc)->_mxs_op_mode.set(0);
|
||||
return get_instance<SagetechMXS>(desc)->_mxs_op_mode.commit();
|
||||
|
||||
} else if (!strcmp(opmode, "on") || !strcmp(opmode, "1")) {
|
||||
get_instance()->_mxs_op_mode.set(1);
|
||||
return get_instance()->_mxs_op_mode.commit();
|
||||
get_instance<SagetechMXS>(desc)->_mxs_op_mode.set(1);
|
||||
return get_instance<SagetechMXS>(desc)->_mxs_op_mode.commit();
|
||||
|
||||
} else if (!strcmp(opmode, "stby") || !strcmp(opmode, "2")) {
|
||||
get_instance()->_mxs_op_mode.set(2);
|
||||
return get_instance()->_mxs_op_mode.commit();
|
||||
get_instance<SagetechMXS>(desc)->_mxs_op_mode.set(2);
|
||||
return get_instance<SagetechMXS>(desc)->_mxs_op_mode.commit();
|
||||
|
||||
} else if (!strcmp(opmode, "alt") || !strcmp(opmode, "3")) {
|
||||
get_instance()->_mxs_op_mode.set(3);
|
||||
return get_instance()->_mxs_op_mode.commit();
|
||||
get_instance<SagetechMXS>(desc)->_mxs_op_mode.set(3);
|
||||
return get_instance<SagetechMXS>(desc)->_mxs_op_mode.commit();
|
||||
|
||||
} else {
|
||||
print_usage("Invalid Op Mode");
|
||||
@@ -200,13 +202,13 @@ int SagetechMXS::custom_command(int argc, char *argv[])
|
||||
|
||||
sqk = atoi(squawk);
|
||||
|
||||
if (!get_instance()->check_valid_squawk(sqk)) {
|
||||
if (!get_instance<SagetechMXS>(desc)->check_valid_squawk(sqk)) {
|
||||
print_usage("Invalid Squawk");
|
||||
return PX4_ERROR;
|
||||
|
||||
} else {
|
||||
get_instance()->_adsb_squawk.set(sqk);
|
||||
return get_instance()->_adsb_squawk.commit();
|
||||
get_instance<SagetechMXS>(desc)->_adsb_squawk.set(sqk);
|
||||
return get_instance<SagetechMXS>(desc)->_adsb_squawk.commit();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -274,7 +276,7 @@ void SagetechMXS::Run()
|
||||
// Thread Stop
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -65,9 +65,11 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class SagetechMXS : public ModuleBase<SagetechMXS>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class SagetechMXS : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
SagetechMXS(const char *port);
|
||||
~SagetechMXS() override;
|
||||
|
||||
|
||||
@@ -38,6 +38,10 @@ if DRIVERS_UAVCANNODE
|
||||
bool "Include hardpoint commands"
|
||||
default n
|
||||
|
||||
config UAVCANNODE_GNSS_FIX2
|
||||
bool "Include GNSS Fix2 publisher"
|
||||
default y
|
||||
|
||||
config UAVCANNODE_HYGROMETER_MEASUREMENT
|
||||
bool "Include hygrometer measurement"
|
||||
default n
|
||||
|
||||
@@ -49,6 +49,8 @@
|
||||
#include <ctype.h>
|
||||
#include <string.h>
|
||||
|
||||
ModuleBase::Descriptor UWB_SR150::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
// Timeout between bytes. If there is more time than this between bytes, then this driver assumes
|
||||
// that it is the boundary between messages.
|
||||
// See uwb_sr150::run() for more detailed explanation.
|
||||
@@ -117,7 +119,7 @@ void UWB_SR150::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -248,8 +250,8 @@ int UWB_SR150::task_spawn(int argc, char *argv[])
|
||||
UWB_SR150 *instance = new UWB_SR150(device_name);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleOnInterval(5000_us);
|
||||
|
||||
@@ -262,15 +264,15 @@ int UWB_SR150::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int uwb_sr150_main(int argc, char *argv[])
|
||||
{
|
||||
return UWB_SR150::main(argc, argv);
|
||||
return ModuleBase::main(UWB_SR150::desc, argc, argv);
|
||||
}
|
||||
|
||||
void UWB_SR150::parameters_update()
|
||||
|
||||
@@ -84,9 +84,11 @@ typedef struct {
|
||||
uint8_t stop; // Should be 0x1B
|
||||
} __attribute__((packed)) distance_msg_t;
|
||||
|
||||
class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class UWB_SR150 : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
UWB_SR150(const char *port);
|
||||
~UWB_SR150();
|
||||
|
||||
|
||||
@@ -33,6 +33,8 @@
|
||||
|
||||
#include "voxl2_io.hpp"
|
||||
|
||||
ModuleBase::Descriptor Voxl2IO::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
|
||||
Voxl2IO::Voxl2IO() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)),
|
||||
@@ -558,7 +560,7 @@ void Voxl2IO::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -603,35 +605,35 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
argv++;
|
||||
|
||||
while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'v':
|
||||
PX4_INFO("Verbose mode enabled");
|
||||
get_instance()->_debug = true;
|
||||
get_instance<Voxl2IO>(desc)->_debug = true;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
PX4_INFO("M0065 PWM outputs disabled");
|
||||
get_instance()->_outputs_disabled = true;
|
||||
get_instance<Voxl2IO>(desc)->_outputs_disabled = true;
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
PX4_INFO("M0065 using external RC");
|
||||
get_instance()->_rc_mode = RC_MODE::EXTERNAL;
|
||||
get_instance<Voxl2IO>(desc)->_rc_mode = RC_MODE::EXTERNAL;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
if (valid_port(atoi(myoptarg))) {
|
||||
snprintf(get_instance()->_device, 2, "%s", myoptarg);
|
||||
snprintf(get_instance<Voxl2IO>(desc)->_device, 2, "%s", myoptarg);
|
||||
|
||||
} else {
|
||||
PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg);
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
@@ -651,8 +653,8 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -754,7 +756,7 @@ int Voxl2IO::custom_command(int argc, char *argv[])
|
||||
|
||||
/* start if not running */
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
return Voxl2IO::task_spawn(argc, argv);
|
||||
}
|
||||
|
||||
@@ -762,27 +764,27 @@ int Voxl2IO::custom_command(int argc, char *argv[])
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
PX4_INFO("Not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return get_instance()->print_status();
|
||||
return get_instance<Voxl2IO>(desc)->print_status();
|
||||
}
|
||||
|
||||
|
||||
if (!strcmp(verb, "calibrate_escs")) {
|
||||
if (get_instance()->_outputs_disabled) {
|
||||
if (get_instance<Voxl2IO>(desc)->_outputs_disabled) {
|
||||
PX4_WARN("Can't calibrate ESCs while outputs are disabled.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return get_instance()->calibrate_escs();
|
||||
return get_instance<Voxl2IO>(desc)->calibrate_escs();
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "enable_debug")) {
|
||||
get_instance()->_debug = true;
|
||||
get_instance<Voxl2IO>(desc)->_debug = true;
|
||||
}
|
||||
|
||||
return print_usage("unknown custom command");
|
||||
@@ -881,5 +883,5 @@ extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]);
|
||||
|
||||
int voxl2_io_main(int argc, char *argv[])
|
||||
{
|
||||
return Voxl2IO::main(argc, argv);
|
||||
return ModuleBase::main(Voxl2IO::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -64,9 +64,11 @@ using namespace device;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Voxl2IO final : public ModuleBase<Voxl2IO>, public OutputModuleInterface
|
||||
class Voxl2IO final : public ModuleBase, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
Voxl2IO();
|
||||
~Voxl2IO() override;
|
||||
|
||||
|
||||
@@ -41,6 +41,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor VTX::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
VTX::VTX(const char *device) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)),
|
||||
@@ -99,7 +101,7 @@ void VTX::Run()
|
||||
static constexpr auto _INTERVAL{50_ms};
|
||||
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -338,7 +340,7 @@ void VTX::handle_uorb()
|
||||
int VTX::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!strcmp(argv[0], "start")) {
|
||||
if (is_running()) {
|
||||
if (is_running(desc)) {
|
||||
return print_usage("already running");
|
||||
}
|
||||
|
||||
@@ -349,7 +351,7 @@ int VTX::custom_command(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_running()) {
|
||||
if (!is_running(desc)) {
|
||||
return print_usage("not running");
|
||||
}
|
||||
|
||||
@@ -393,8 +395,8 @@ int VTX::task_spawn(int argc, char *argv[])
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
@@ -478,5 +480,5 @@ Supported protocols are:
|
||||
|
||||
extern "C" __EXPORT int vtx_main(int argc, char *argv[])
|
||||
{
|
||||
return VTX::main(argc, argv);
|
||||
return ModuleBase::main(VTX::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -56,10 +56,12 @@
|
||||
/**
|
||||
* @author Niklas Hauser <niklas@auterion.com>
|
||||
*/
|
||||
class VTX : public ModuleBase<VTX>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class VTX : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static Descriptor desc;
|
||||
|
||||
VTX(const char *device);
|
||||
virtual ~VTX();
|
||||
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor FakeGps::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
FakeGps::FakeGps(double latitude_deg, double longitude_deg, double altitude_m) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
|
||||
@@ -54,7 +56,7 @@ void FakeGps::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -102,8 +104,8 @@ int FakeGps::task_spawn(int argc, char *argv[])
|
||||
FakeGps *instance = new FakeGps();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -114,8 +116,8 @@ int FakeGps::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -145,5 +147,5 @@ int FakeGps::print_usage(const char *reason)
|
||||
|
||||
extern "C" __EXPORT int fake_gps_main(int argc, char *argv[])
|
||||
{
|
||||
return FakeGps::main(argc, argv);
|
||||
return ModuleBase::main(FakeGps::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -43,9 +43,11 @@
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/sensor_gnss_status.h>
|
||||
|
||||
class FakeGps : public ModuleBase<FakeGps>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class FakeGps : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, double altitude_m = 30.1);
|
||||
|
||||
~FakeGps() override = default;
|
||||
|
||||
@@ -35,6 +35,8 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor FakeImu::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
FakeImu::FakeImu() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
@@ -60,7 +62,7 @@ void FakeImu::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -168,8 +170,8 @@ int FakeImu::task_spawn(int argc, char *argv[])
|
||||
FakeImu *instance = new FakeImu();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -180,8 +182,8 @@ int FakeImu::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -211,5 +213,5 @@ int FakeImu::print_usage(const char *reason)
|
||||
|
||||
extern "C" __EXPORT int fake_imu_main(int argc, char *argv[])
|
||||
{
|
||||
return FakeImu::main(argc, argv);
|
||||
return ModuleBase::main(FakeImu::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -51,9 +51,11 @@
|
||||
# include <uORB/topics/esc_status.h>
|
||||
#endif // FAKE_IMU_FAKE_ESC_STATUS
|
||||
|
||||
class FakeImu : public ModuleBase<FakeImu>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class FakeImu : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
FakeImu();
|
||||
~FakeImu() override = default;
|
||||
|
||||
|
||||
@@ -38,6 +38,8 @@
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
ModuleBase::Descriptor FakeMagnetometer::desc{task_spawn, custom_command, print_usage};
|
||||
|
||||
FakeMagnetometer::FakeMagnetometer() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
@@ -56,7 +58,7 @@ void FakeMagnetometer::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
exit_and_cleanup(desc);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -94,8 +96,8 @@ int FakeMagnetometer::task_spawn(int argc, char *argv[])
|
||||
FakeMagnetometer *instance = new FakeMagnetometer();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
desc.object.store(instance);
|
||||
desc.task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
@@ -106,8 +108,8 @@ int FakeMagnetometer::task_spawn(int argc, char *argv[])
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
desc.object.store(nullptr);
|
||||
desc.task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
@@ -138,5 +140,5 @@ Requires vehicle_attitude and vehicle_gps_position.
|
||||
|
||||
extern "C" __EXPORT int fake_magnetometer_main(int argc, char *argv[])
|
||||
{
|
||||
return FakeMagnetometer::main(argc, argv);
|
||||
return ModuleBase::main(FakeMagnetometer::desc, argc, argv);
|
||||
}
|
||||
|
||||
@@ -52,9 +52,11 @@
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
class FakeMagnetometer : public ModuleBase<FakeMagnetometer>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
class FakeMagnetometer : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
static Descriptor desc;
|
||||
|
||||
FakeMagnetometer();
|
||||
~FakeMagnetometer() override = default;
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user