Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

refactor pwm_out_sim: use mixer_module and run on work queue

Browse Source 
Tested with SITL + HITL
This commit is contained in:
Beat Küng
2019-10-15 10:58:20 +02:00
parent 2837152983
commit 349469cf75
10 changed files with 137 additions and 572 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.interface
+1 -1
View File
@@ -84,7 +84,7 @@ then
if [ $OUTPUT_MODE = hil -o $OUTPUT_MODE = sim ]
then
if ! pwm_out_sim start
if ! pwm_out_sim start -m $OUTPUT_MODE
then
# Error tune.
tune_control play -t 2
Tools/uorb_graph/create.py
-1
View File
@@ -227,7 +227,6 @@ class Graph(object):
('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
('pwm_out_sim', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
]
src/drivers/drv_pwm_output.h
+1 -6
View File
@@ -66,14 +66,9 @@ __BEGIN_DECLS
struct pwm_output_values {
uint32_t channel_count;
uint16_t values[16];
uint16_t values[PWM_OUTPUT_MAX_CHANNELS];
};
/**
* Maximum number of PWM output channels supported by the device.
*/
//#define PWM_OUTPUT_MAX_CHANNELS 16
/* Use defaults unless the board override the defaults by providing
* PX4_PWM_ALTERNATE_RANGES and a replacement set of
* constants
src/drivers/dshot/dshot.cpp
+1 -4
View File
@@ -121,9 +121,6 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static DShotOutput *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@@ -197,7 +194,7 @@ private:
int requestESCInfo();
MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false};
MixingOutput _mixing_output{DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
Telemetry *_telemetry{nullptr};
static char _telemetry_device[20];
src/drivers/pwm_out_sim/CMakeLists.txt
+2
View File
@@ -37,4 +37,6 @@ px4_add_module(
PWMSim.cpp
DEPENDS
mixer
mixer_module
output_limit
)
src/drivers/pwm_out_sim/PWMSim.cpp
+78 -457
View File
File diff suppressed because it is too large Load Diff
src/drivers/pwm_out_sim/PWMSim.hpp
+13 -75
View File
@@ -33,112 +33,50 @@
#pragma once
#include <string.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mixer/mixer.h>
#include <perf/perf_counter.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_config.h>
#include <px4_module.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
{
static constexpr uint32_t PWM_SIM_DISARMED_MAGIC = 900;
static constexpr uint32_t PWM_SIM_FAILSAFE_MAGIC = 600;
static constexpr uint32_t PWM_SIM_PWM_MIN_MAGIC = 1000;
static constexpr uint32_t PWM_SIM_PWM_MAX_MAGIC = 2000;
public:
enum Mode {
MODE_8PWM,
MODE_16PWM,
MODE_NONE
};
PWMSim();
virtual ~PWMSim();
PWMSim(bool hil_mode_enabled);
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static PWMSim *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
void Run() override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int set_pwm_rate(unsigned rate);
int set_mode(Mode mode);
Mode get_mode() { return _mode; }
void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
static constexpr unsigned MAX_ACTUATORS = 16;
static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600;
static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000;
static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000;
Mode _mode{MODE_NONE};
MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
int _update_rate{400};
int _current_update_rate{0};
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
unsigned _poll_fds_num{0};
int _armed_sub{-1};
actuator_outputs_s _actuator_outputs {};
uORB::Publication<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
orb_advert_t _mixer_status{nullptr};
unsigned _num_outputs{0};
unsigned _pwm_min[MAX_ACTUATORS] {};
unsigned _pwm_max[MAX_ACTUATORS] {};
uint32_t _groups_required{0};
uint32_t _groups_subscribed{0};
bool _armed{false};
bool _lockdown{false};
bool _failsafe{false};
MixerGroup *_mixers{nullptr};
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
Mixer::Airmode _airmode{Mixer::Airmode::disabled}; ///< multicopter air-mode
perf_counter_t _perf_control_latency;
static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
void subscribe();
void update_params();
};
src/drivers/px4fmu/fmu.cpp
+24 -24
View File
@@ -123,9 +123,6 @@ public:
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static PX4FMU *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@@ -163,7 +160,10 @@ public:
unsigned num_outputs, unsigned num_control_groups_updated) override;
private:
MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, true};
static constexpr int FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
static_assert(FMU_MAX_ACTUATORS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails");
MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
Mode _mode{MODE_NONE};
@@ -497,7 +497,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
for (unsigned pass = 0; pass < 2; pass++) {
/* We should note that group is iterated over from 0 to MAX_ACTUATORS.
/* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
* This allows for the ideal worlds situation: 1 channel per group
* configuration.
*
@@ -511,7 +511,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
* rate and mode. (See rates above.)
*/
for (unsigned group = 0; group < MAX_ACTUATORS; group++) {
for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
@@ -606,7 +606,7 @@ PX4FMU::update_pwm_rev_mask()
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
char pname[16];
/* fill the channel reverse mask from parameters */
@@ -630,7 +630,7 @@ PX4FMU::update_pwm_trims()
return;
}
int16_t values[MAX_ACTUATORS] = {};
int16_t values[FMU_MAX_ACTUATORS] = {};
const char *pname_format;
@@ -645,7 +645,7 @@ PX4FMU::update_pwm_trims()
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
char pname[16];
/* fill the struct from parameters */
@@ -661,7 +661,7 @@ PX4FMU::update_pwm_trims()
}
/* copy the trim values to the mixer offsets */
unsigned n_out = _mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
unsigned n_out = _mixing_output.mixers()->set_trims(values, FMU_MAX_ACTUATORS);
PX4_DEBUG("set %d trims", n_out);
}
@@ -893,7 +893,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -926,11 +926,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.failsafeValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
@@ -938,7 +938,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -969,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
if (_mixing_output.disarmedValue(i) > 0) {
_num_disarmed_set++;
}
@@ -981,11 +981,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.disarmedValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
@@ -993,7 +993,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -1025,11 +1025,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.minValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
break;
}
@@ -1038,7 +1038,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
ret = -EINVAL;
break;
}
@@ -1063,11 +1063,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.maxValue(i);
}
pwm->channel_count = MAX_ACTUATORS;
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
break;
}
@@ -1076,7 +1076,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > MAX_ACTUATORS) {
if (pwm->channel_count > FMU_MAX_ACTUATORS) {
PX4_DEBUG("error: too many trim values: %d", pwm->channel_count);
ret = -EINVAL;
break;
src/lib/mixer_module/mixer_module.cpp
+9 -2
View File
@@ -39,7 +39,8 @@
using namespace time_literals;
MixingOutput::MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy,
MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface,
SchedulingPolicy scheduling_policy,
bool support_esc_calibration, bool ramp_up)
: ModuleParams(&interface),
_control_subs{
@@ -50,6 +51,7 @@ MixingOutput::MixingOutput(OutputModuleInterface &interface, SchedulingPolicy sc
},
_scheduling_policy(scheduling_policy),
_support_esc_calibration(support_esc_calibration),
_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS),
_interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
{
@@ -240,6 +242,11 @@ bool MixingOutput::update()
// check arming state
if (_armed_sub.update(&_armed)) {
_armed.in_esc_calibration_mode &= _support_esc_calibration;
if (_ignore_lockdown) {
_armed.lockdown = false;
}
/* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode);
@@ -275,7 +282,7 @@ bool MixingOutput::update()
/* do mixing */
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, MAX_ACTUATORS);
const unsigned mixed_num_outputs = _mixers->mix(outputs, _max_num_outputs);
/* the output limit call takes care of out of band errors, NaN and constrains */
uint16_t output_limited[MAX_ACTUATORS] {};
src/lib/mixer_module/mixer_module.hpp
+8 -2
View File
@@ -34,6 +34,7 @@
#pragma once
#include <board_config.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mixer/mixer.h>
#include <lib/perf/perf_counter.h>
#include <lib/output_limit/output_limit.h>
@@ -59,7 +60,7 @@
class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams
{
public:
static constexpr int MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS;
OutputModuleInterface(const char *name, const px4::wq_config_t &config)
: px4::ScheduledWorkItem(name, config), ModuleParams(nullptr) {}
@@ -90,12 +91,13 @@ public:
/**
* Contructor
* @param max_num_outputs maximum number of supported outputs
* @param interface Parent module for scheduling, parameter updates and callbacks
* @param scheduling_policy
* @param support_esc_calibration true if the output module supports ESC calibration via max, then min setting
* @param ramp_up true if motor ramp up from disarmed to min upon arming is wanted
*/
MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy,
MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface, SchedulingPolicy scheduling_policy,
bool support_esc_calibration, bool ramp_up = true);
~MixingOutput();
@@ -163,6 +165,8 @@ public:
*/
int reorderedMotorIndex(int index);
void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; }
protected:
void updateParams() override;
@@ -232,6 +236,7 @@ private:
bool _safety_off{false}; ///< State of the safety button from the subscribed _safety_sub topic
bool _throttle_armed{false};
bool _ignore_lockdown{false}; ///< if true, ignore the _armed.lockdown flag (for HIL outputs)
MixerGroup *_mixers{nullptr};
uint32_t _groups_required{0};
@@ -241,6 +246,7 @@ private:
const bool _support_esc_calibration;
bool _wq_switched{false};
const uint8_t _max_num_outputs;
OutputModuleInterface &_interface;