mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
drivers/pwm_out: remove custom module boilerplate (dual output bank handling)
This commit is contained in:
Daniel Agar
parent
ce337a3d80
commit
baa05b2631
@ -35,28 +35,14 @@
|
||||
|
||||
#include <px4_platform_common/sem.hpp>
|
||||
|
||||
pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
|
||||
static px4::atomic_bool _require_arming[PWM_OUT_MAX_INSTANCES] {};
|
||||
|
||||
static bool is_running()
|
||||
PWMOut::PWMOut() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
for (auto &obj : _objects) {
|
||||
if (obj.load() != nullptr) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
_pwm_mask = ((1u << DIRECT_PWM_OUTPUT_CHANNELS) - 1);
|
||||
_mixing_output.setMaxNumOutputs(DIRECT_PWM_OUTPUT_CHANNELS);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
PWMOut::PWMOut(int instance, uint8_t output_base) :
|
||||
OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
|
||||
_instance(instance),
|
||||
_output_base(output_base),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
|
||||
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
|
||||
{
|
||||
// Getting initial parameter values
|
||||
update_params();
|
||||
}
|
||||
|
||||
PWMOut::~PWMOut()
|
||||
@ -68,60 +54,6 @@ PWMOut::~PWMOut()
|
||||
perf_free(_interval_perf);
|
||||
}
|
||||
|
||||
int PWMOut::init()
|
||||
{
|
||||
_num_outputs = FMU_MAX_ACTUATORS;
|
||||
|
||||
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
|
||||
// Getting initial parameter values
|
||||
update_params();
|
||||
|
||||
ScheduleNow();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PWMOut::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) {
|
||||
|
||||
if (instance < PWM_OUT_MAX_INSTANCES) {
|
||||
uint8_t base = instance * MAX_PER_INSTANCE; // TODO: configurable
|
||||
PWMOut *dev = new PWMOut(instance, base);
|
||||
|
||||
if (dev) {
|
||||
_objects[instance].store(dev);
|
||||
|
||||
if (dev->init() != PX4_OK) {
|
||||
PX4_ERR("%d - init failed", instance);
|
||||
delete dev;
|
||||
_objects[instance].store(nullptr);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// only start one instance with dynamic mixing
|
||||
//if (dev->_mixing_output.useDynamicMixing()) {
|
||||
if (true) {
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
} else {
|
||||
// This hardware platform does not support
|
||||
// this many devices, set the storage to
|
||||
// a sane default
|
||||
_objects[instance].store(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
bool PWMOut::update_pwm_out_state(bool on)
|
||||
{
|
||||
if (on && !_pwm_initialized && _pwm_mask != 0) {
|
||||
@ -189,7 +121,6 @@ bool PWMOut::update_pwm_out_state(bool on)
|
||||
}
|
||||
}
|
||||
|
||||
_require_arming[_instance].store(false);
|
||||
up_pwm_servo_arm(on, _pwm_mask);
|
||||
return true;
|
||||
}
|
||||
@ -205,8 +136,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
outputs[i] = 0;
|
||||
}
|
||||
|
||||
if (_pwm_mask & (1 << (i + _output_base))) {
|
||||
up_pwm_servo_set(_output_base + i, outputs[i]);
|
||||
if (_pwm_mask & (1 << i)) {
|
||||
up_pwm_servo_set(i, outputs[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -227,7 +158,7 @@ void PWMOut::Run()
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
//exit_and_cleanup();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -239,8 +170,7 @@ void PWMOut::Run()
|
||||
/* update PWM status if armed or if disarmed PWM values are set */
|
||||
bool pwm_on = true;
|
||||
|
||||
if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
|
||||
|
||||
if (_pwm_on != pwm_on) {
|
||||
if (update_pwm_out_state(pwm_on)) {
|
||||
_pwm_on = pwm_on;
|
||||
}
|
||||
@ -262,6 +192,21 @@ void PWMOut::Run()
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
int PWMOut::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
PWMOut *instance = new PWMOut();
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void PWMOut::update_params()
|
||||
{
|
||||
uint32_t previously_set_functions = 0;
|
||||
@ -323,8 +268,6 @@ int PWMOut::custom_command(int argc, char *argv[])
|
||||
|
||||
int PWMOut::print_status()
|
||||
{
|
||||
PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate);
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
perf_print_counter(_interval_perf);
|
||||
_mixing_output.printStatus();
|
||||
@ -366,13 +309,6 @@ This module is responsible for driving the output pins. For boards without a sep
|
||||
(eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the
|
||||
px4io driver is used for main ones.
|
||||
|
||||
It listens on the actuator_controls topics, does the mixing and writes the PWM outputs.
|
||||
|
||||
On startup, the module tries to occupy all available pins for PWM/Oneshot output.
|
||||
It skips all pins already in use (e.g. by a camera trigger module).
|
||||
|
||||
### Implementation
|
||||
By default the module runs on a work queue with a callback on the uORB actuator_controls topic.
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("pwm_out", "driver");
|
||||
@ -384,104 +320,5 @@ By default the module runs on a work queue with a callback on the uORB actuator_
|
||||
|
||||
extern "C" __EXPORT int pwm_out_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
|
||||
return PWMOut::print_usage();
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "start") == 0) {
|
||||
|
||||
if (is_running()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ret = 0;
|
||||
|
||||
PWMOut::lock_module();
|
||||
|
||||
ret = PWMOut::task_spawn(argc - 1, argv + 1);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("start failed (%i)", ret);
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
return ret;
|
||||
|
||||
} else if (strcmp(argv[1], "status") == 0) {
|
||||
if (PWMOut::trylock_module()) {
|
||||
|
||||
unsigned count = 0;
|
||||
|
||||
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
|
||||
if (_objects[i].load()) {
|
||||
PX4_INFO_RAW("\n");
|
||||
_objects[i].load()->print_status();
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
|
||||
if (count == 0) {
|
||||
PX4_INFO("not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_WARN("module locked, try again later");
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else if (strcmp(argv[1], "stop") == 0) {
|
||||
PWMOut::lock_module();
|
||||
|
||||
if (argc > 2) {
|
||||
int instance = atoi(argv[2]);
|
||||
|
||||
if (instance >= 0 && instance < PWM_OUT_MAX_INSTANCES) {
|
||||
PX4_INFO("stopping instance %d", instance);
|
||||
PWMOut *inst = _objects[instance].load();
|
||||
|
||||
if (inst) {
|
||||
inst->request_stop();
|
||||
px4_usleep(20000); // 20 ms
|
||||
delete inst;
|
||||
_objects[instance].store(nullptr);
|
||||
}
|
||||
} else {
|
||||
PX4_ERR("invalid instance %d", instance);
|
||||
}
|
||||
|
||||
} else {
|
||||
// otherwise stop everything
|
||||
bool was_running = false;
|
||||
|
||||
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
|
||||
PWMOut *inst = _objects[i].load();
|
||||
|
||||
if (inst) {
|
||||
PX4_INFO("stopping pwm_out instance %d", i);
|
||||
was_running = true;
|
||||
inst->request_stop();
|
||||
px4_usleep(20000); // 20 ms
|
||||
delete inst;
|
||||
_objects[i].store(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
if (!was_running) {
|
||||
PX4_WARN("not running");
|
||||
}
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
PWMOut::lock_module(); // Lock here, as the method could access _object.
|
||||
int ret = PWMOut::custom_command(argc - 1, argv + 1);
|
||||
PWMOut::unlock_module();
|
||||
|
||||
return ret;
|
||||
return PWMOut::main(argc, argv);
|
||||
}
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
@ -49,26 +48,16 @@
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};
|
||||
extern pthread_mutex_t pwm_out_module_mutex;
|
||||
|
||||
class PWMOut : public OutputModuleInterface
|
||||
class PWMOut final : public ModuleBase<PWMOut>, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
PWMOut() = delete;
|
||||
explicit PWMOut(int instance = 0, uint8_t output_base = 0);
|
||||
|
||||
virtual ~PWMOut();
|
||||
PWMOut();
|
||||
~PWMOut() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
@ -79,71 +68,31 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
void Run() override;
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status();
|
||||
|
||||
bool should_exit() const { return _task_should_exit.load(); }
|
||||
void request_stop() { _task_should_exit.store(true); }
|
||||
|
||||
static void lock_module() { pthread_mutex_lock(&pwm_out_module_mutex); }
|
||||
static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); }
|
||||
static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); }
|
||||
|
||||
static int test(const char *dev);
|
||||
|
||||
int init();
|
||||
|
||||
uint32_t get_pwm_mask() const { return _pwm_mask; }
|
||||
void set_pwm_mask(uint32_t mask) { _pwm_mask = mask; }
|
||||
uint32_t get_alt_rate_channels() const { return _pwm_alt_rate_channels; }
|
||||
unsigned get_alt_rate() const { return _pwm_alt_rate; }
|
||||
unsigned get_default_rate() const { return _pwm_default_rate; }
|
||||
int print_status() override;
|
||||
|
||||
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
private:
|
||||
static constexpr int FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
|
||||
static_assert(FMU_MAX_ACTUATORS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails");
|
||||
void Run() override;
|
||||
|
||||
px4::atomic_bool _task_should_exit{false};
|
||||
void update_params();
|
||||
bool update_pwm_out_state(bool on);
|
||||
|
||||
const int _instance;
|
||||
const uint32_t _output_base;
|
||||
|
||||
static const int MAX_PER_INSTANCE{8};
|
||||
|
||||
MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
|
||||
|
||||
unsigned _pwm_default_rate{50};
|
||||
unsigned _pwm_alt_rate{50};
|
||||
uint32_t _pwm_alt_rate_channels{0};
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true};
|
||||
|
||||
int _timer_rates[MAX_IO_TIMERS] {};
|
||||
|
||||
int _current_update_rate{0};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
unsigned _num_outputs{0};
|
||||
unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
|
||||
|
||||
bool _pwm_on{false};
|
||||
uint32_t _pwm_mask{0};
|
||||
bool _pwm_initialized{false};
|
||||
bool _first_param_update{true};
|
||||
|
||||
unsigned _num_disarmed_set{0};
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
|
||||
bool update_pwm_out_state(bool on);
|
||||
|
||||
void update_params();
|
||||
|
||||
PWMOut(const PWMOut &) = delete;
|
||||
PWMOut operator=(const PWMOut &) = delete;
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user