mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
refactor pwm_out_sim: use mixer_module and run on work queue
Tested with SITL + HITL
This commit is contained in:
Beat Küng
parent
2837152983
commit
349469cf75
@ -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
|
||||
|
||||
@ -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\]$'),
|
||||
]
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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];
|
||||
|
||||
@ -37,4 +37,6 @@ px4_add_module(
|
||||
PWMSim.cpp
|
||||
DEPENDS
|
||||
mixer
|
||||
mixer_module
|
||||
output_limit
|
||||
)
|
||||
|
||||
@ -33,326 +33,57 @@
|
||||
|
||||
#include "PWMSim.hpp"
|
||||
|
||||
#include <px4_time.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_getopt.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
|
||||
PWMSim::PWMSim() :
|
||||
extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]);
|
||||
|
||||
PWMSim::PWMSim(bool hil_mode_enabled) :
|
||||
CDev(PWM_OUTPUT0_DEVICE_PATH),
|
||||
_perf_control_latency(perf_alloc(PC_ELAPSED, "pwm_out_sim control latency"))
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
|
||||
_pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC;
|
||||
_pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC;
|
||||
}
|
||||
_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC);
|
||||
_mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC);
|
||||
_mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC);
|
||||
_mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
|
||||
|
||||
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||
_control_topics[2] = ORB_ID(actuator_controls_2);
|
||||
_control_topics[3] = ORB_ID(actuator_controls_3);
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
_mixing_output.setIgnoreLockdown(hil_mode_enabled);
|
||||
|
||||
CDev::init();
|
||||
|
||||
// default to MODE_16PWM
|
||||
set_mode(MODE_16PWM);
|
||||
}
|
||||
|
||||
PWMSim::~PWMSim()
|
||||
{
|
||||
perf_free(_perf_control_latency);
|
||||
}
|
||||
|
||||
int
|
||||
PWMSim::set_mode(Mode mode)
|
||||
{
|
||||
/*
|
||||
* Configure for PWM output.
|
||||
*
|
||||
* Note that regardless of the configured mode, the task is always
|
||||
* listening and mixing; the mode just selects which of the channels
|
||||
* are presented on the output pins.
|
||||
*/
|
||||
switch (mode) {
|
||||
case MODE_8PWM:
|
||||
/* multi-port as 8 PWM outs */
|
||||
_update_rate = 400; /* default output rate */
|
||||
_num_outputs = 8;
|
||||
break;
|
||||
|
||||
case MODE_16PWM:
|
||||
/* multi-port as 16 PWM outs */
|
||||
_update_rate = 400; /* default output rate */
|
||||
_num_outputs = 16;
|
||||
break;
|
||||
|
||||
case MODE_NONE:
|
||||
/* disable servo outputs and set a very low update rate */
|
||||
_update_rate = 10;
|
||||
_num_outputs = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_mode = mode;
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
PWMSim::set_pwm_rate(unsigned rate)
|
||||
{
|
||||
if ((rate > 500) || (rate < 10)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_update_rate = rate;
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
PWMSim::subscribe()
|
||||
PWMSim::Run()
|
||||
{
|
||||
/* subscribe/unsubscribe to required actuator control groups */
|
||||
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
|
||||
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
|
||||
_poll_fds_num = 0;
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (sub_groups & (1 << i)) {
|
||||
PX4_DEBUG("subscribe to actuator_controls_%d", i);
|
||||
_control_subs[i] = orb_subscribe(_control_topics[i]);
|
||||
}
|
||||
|
||||
if (unsub_groups & (1 << i)) {
|
||||
PX4_DEBUG("unsubscribe from actuator_controls_%d", i);
|
||||
orb_unsubscribe(_control_subs[i]);
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
|
||||
if (_control_subs[i] >= 0) {
|
||||
_poll_fds[_poll_fds_num].fd = _control_subs[i];
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num++;
|
||||
}
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void PWMSim::update_params()
|
||||
{
|
||||
// multicopter air-mode
|
||||
param_t param_handle = param_find("MC_AIRMODE");
|
||||
_mixing_output.update();
|
||||
|
||||
if (param_handle != PARAM_INVALID) {
|
||||
param_get(param_handle, &_airmode);
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
|
||||
_mixing_output.updateSubscriptions(true);
|
||||
}
|
||||
|
||||
void
|
||||
PWMSim::run()
|
||||
PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
|
||||
unsigned num_control_groups_updated)
|
||||
{
|
||||
/* force a reset of the update rate */
|
||||
_current_update_rate = 0;
|
||||
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
|
||||
update_params();
|
||||
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
/* loop until killed */
|
||||
while (!should_exit()) {
|
||||
|
||||
if (_groups_subscribed != _groups_required) {
|
||||
subscribe();
|
||||
_groups_subscribed = _groups_required;
|
||||
}
|
||||
|
||||
/* handle update rate changes */
|
||||
if (_current_update_rate != _update_rate) {
|
||||
int update_rate_in_ms = int(1000 / _update_rate);
|
||||
|
||||
if (update_rate_in_ms < 2) {
|
||||
update_rate_in_ms = 2;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
orb_set_interval(_control_subs[i], update_rate_in_ms);
|
||||
}
|
||||
}
|
||||
|
||||
// up_pwm_servo_set_rate(_update_rate);
|
||||
_current_update_rate = _update_rate;
|
||||
}
|
||||
|
||||
if (_mixers) {
|
||||
_mixers->set_airmode(_airmode);
|
||||
}
|
||||
|
||||
/* this can happen during boot, but after the sleep its likely resolved */
|
||||
if (_poll_fds_num == 0) {
|
||||
px4_sleep(1);
|
||||
|
||||
PX4_DEBUG("no valid fds");
|
||||
continue;
|
||||
}
|
||||
|
||||
/* sleep waiting for data, but no more than a second */
|
||||
int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000);
|
||||
|
||||
/* this would be bad... */
|
||||
if (ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ret == 0) {
|
||||
// timeout
|
||||
continue;
|
||||
}
|
||||
|
||||
/* get controls for required topics */
|
||||
unsigned poll_id = 0;
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
if (_poll_fds[poll_id].revents & POLLIN) {
|
||||
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
|
||||
}
|
||||
|
||||
poll_id++;
|
||||
}
|
||||
}
|
||||
|
||||
/* can we mix? */
|
||||
/* We also publish if not armed, this way we make sure SITL gets feedback. */
|
||||
if (_mixers != nullptr) {
|
||||
|
||||
/* do mixing */
|
||||
_actuator_outputs.noutputs = _mixers->mix(&_actuator_outputs.output[0], _num_outputs);
|
||||
|
||||
/* disable unused ports by setting their output to NaN */
|
||||
const size_t actuator_outputs_size = sizeof(_actuator_outputs.output) / sizeof(_actuator_outputs.output[0]);
|
||||
|
||||
for (size_t i = _actuator_outputs.noutputs; i < actuator_outputs_size; i++) {
|
||||
_actuator_outputs.output[i] = NAN;
|
||||
}
|
||||
|
||||
/* iterate actuators */
|
||||
for (unsigned i = 0; i < _actuator_outputs.noutputs; i++) {
|
||||
/* last resort: catch NaN, INF and out-of-band errors */
|
||||
const bool sane_mixer_output = PX4_ISFINITE(_actuator_outputs.output[i]) &&
|
||||
_actuator_outputs.output[i] >= -1.0f &&
|
||||
_actuator_outputs.output[i] <= 1.0f;
|
||||
|
||||
if (_armed && sane_mixer_output) {
|
||||
/* scale for PWM output 1000 - 2000us */
|
||||
_actuator_outputs.output[i] = 1500 + (500 * _actuator_outputs.output[i]);
|
||||
_actuator_outputs.output[i] = math::constrain(_actuator_outputs.output[i], (float)_pwm_min[i], (float)_pwm_max[i]);
|
||||
|
||||
} else {
|
||||
/* Disarmed or insane value - set disarmed pwm value
|
||||
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||
* spinning motors. It would be deadly in flight. */
|
||||
_actuator_outputs.output[i] = PWM_SIM_DISARMED_MAGIC;
|
||||
}
|
||||
}
|
||||
|
||||
/* overwrite outputs in case of force_failsafe */
|
||||
if (_failsafe) {
|
||||
for (size_t i = 0; i < _actuator_outputs.noutputs; i++) {
|
||||
_actuator_outputs.output[i] = PWM_SIM_FAILSAFE_MAGIC;
|
||||
}
|
||||
}
|
||||
|
||||
/* overwrite outputs in case of lockdown */
|
||||
if (_lockdown) {
|
||||
for (size_t i = 0; i < _actuator_outputs.noutputs; i++) {
|
||||
_actuator_outputs.output[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/* publish mixer status */
|
||||
MultirotorMixer::saturation_status saturation_status;
|
||||
saturation_status.value = _mixers->get_saturation_status();
|
||||
|
||||
if (saturation_status.flags.valid) {
|
||||
multirotor_motor_limits_s motor_limits;
|
||||
motor_limits.timestamp = hrt_absolute_time();
|
||||
motor_limits.saturation_status = saturation_status.value;
|
||||
|
||||
int instance;
|
||||
orb_publish_auto(ORB_ID(multirotor_motor_limits), &_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
_actuator_outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
_outputs_pub.publish(_actuator_outputs);
|
||||
|
||||
|
||||
// use first valid timestamp_sample for latency tracking
|
||||
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
const bool required = _groups_required & (1 << i);
|
||||
const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample;
|
||||
|
||||
if (required && (timestamp_sample > 0)) {
|
||||
perf_set_elapsed(_perf_control_latency, _actuator_outputs.timestamp - timestamp_sample);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* how about an arming update? */
|
||||
bool updated;
|
||||
|
||||
orb_check(_armed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
actuator_armed_s aa = {};
|
||||
|
||||
if (orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa) == PX4_OK) {
|
||||
/* do not obey the lockdown value, as lockdown is for PWMSim. Only obey manual lockdown */
|
||||
_armed = aa.armed;
|
||||
_failsafe = aa.force_failsafe;
|
||||
_lockdown = aa.manual_lockdown;
|
||||
}
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
update_params();
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] >= 0) {
|
||||
orb_unsubscribe(_control_subs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
orb_unsubscribe(_armed_sub);
|
||||
}
|
||||
|
||||
int
|
||||
PWMSim::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
|
||||
{
|
||||
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
||||
|
||||
input = controls[control_group].control[control_index];
|
||||
|
||||
return 0;
|
||||
// nothing to do, as we are only interested in the actuator_outputs topic publication
|
||||
}
|
||||
|
||||
int
|
||||
@ -364,20 +95,17 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
switch (cmd) {
|
||||
case PWM_SERVO_ARM:
|
||||
// up_pwm_servo_arm(true);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_DISARM:
|
||||
// up_pwm_servo_arm(false);
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_MIN_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < pwm->channel_count; i++) {
|
||||
|
||||
if (i < MAX_ACTUATORS) {
|
||||
_pwm_min[i] = pwm->values[i];
|
||||
if (i < OutputModuleInterface::MAX_ACTUATORS) {
|
||||
_mixing_output.minValue(i) = pwm->values[i];
|
||||
}
|
||||
}
|
||||
|
||||
@ -388,8 +116,8 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < pwm->channel_count; i++) {
|
||||
if (i < MAX_ACTUATORS) {
|
||||
_pwm_max[i] = pwm->values[i];
|
||||
if (i < OutputModuleInterface::MAX_ACTUATORS) {
|
||||
_mixing_output.maxValue(i) = pwm->values[i];
|
||||
}
|
||||
}
|
||||
|
||||
@ -397,20 +125,18 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
case PWM_SERVO_SET_UPDATE_RATE:
|
||||
// PWMSim always outputs at the alternate (usually faster) rate
|
||||
set_pwm_rate(arg);
|
||||
// PWMSim does not limit the update rate
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
|
||||
// PWMSim always outputs at the alternate (usually faster) rate
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
|
||||
*(uint32_t *)arg = 400;
|
||||
*(uint32_t *)arg = 9999;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_UPDATE_RATE:
|
||||
*(uint32_t *)arg = _current_update_rate;
|
||||
*(uint32_t *)arg = 9999;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
|
||||
@ -420,70 +146,55 @@ PWMSim::ioctl(device::file_t *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 < _num_outputs; i++) {
|
||||
pwm->values[i] = PWM_SIM_FAILSAFE_MAGIC;
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.failsafeValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_DISARMED_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = PWM_SIM_DISARMED_MAGIC;
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.disarmedValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MIN_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = PWM_SIM_PWM_MIN_MAGIC;
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.minValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_TRIM_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2;
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = (_mixing_output.maxValue(i) + _mixing_output.minValue(i)) / 2;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET_MAX_PWM: {
|
||||
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
|
||||
|
||||
for (unsigned i = 0; i < _num_outputs; i++) {
|
||||
pwm->values[i] = PWM_SIM_PWM_MAX_MAGIC;
|
||||
}
|
||||
|
||||
pwm->channel_count = _num_outputs;
|
||||
break;
|
||||
}
|
||||
|
||||
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
|
||||
|
||||
unsigned channel = cmd - PWM_SERVO_GET(0);
|
||||
|
||||
if (channel >= _num_outputs) {
|
||||
ret = -EINVAL;
|
||||
|
||||
} else {
|
||||
/* fetch a current PWM value */
|
||||
*(servo_position_t *)arg = _actuator_outputs.output[channel];
|
||||
for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
|
||||
pwm->values[i] = _mixing_output.maxValue(i);
|
||||
}
|
||||
|
||||
pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -497,53 +208,17 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
case PWM_SERVO_GET_COUNT:
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
if (_mode == MODE_16PWM) {
|
||||
*(unsigned *)arg = 16;
|
||||
|
||||
} else if (_mode == MODE_8PWM) {
|
||||
|
||||
*(unsigned *)arg = 8;
|
||||
}
|
||||
|
||||
*(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS;
|
||||
break;
|
||||
|
||||
case MIXERIOCRESET:
|
||||
if (_mixers != nullptr) {
|
||||
delete _mixers;
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
}
|
||||
|
||||
_mixing_output.resetMixerThreadSafe();
|
||||
break;
|
||||
|
||||
case MIXERIOCLOADBUF: {
|
||||
const char *buf = (const char *)arg;
|
||||
unsigned buflen = strnlen(buf, 1024);
|
||||
|
||||
if (_mixers == nullptr) {
|
||||
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
||||
}
|
||||
|
||||
if (_mixers == nullptr) {
|
||||
_groups_required = 0;
|
||||
ret = -ENOMEM;
|
||||
|
||||
} else {
|
||||
|
||||
ret = _mixers->load_from_buf(buf, buflen);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("mixer load failed with %d", ret);
|
||||
delete _mixers;
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
ret = -EINVAL;
|
||||
|
||||
} else {
|
||||
_mixers->groups_required(_groups_required);
|
||||
}
|
||||
}
|
||||
|
||||
ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -561,67 +236,38 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
int
|
||||
PWMSim::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("pwm_out_sim",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
|
||||
1100,
|
||||
(px4_main_t)&run_trampoline,
|
||||
nullptr);
|
||||
bool hil_mode = false;
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'm':
|
||||
hil_mode = strcmp(myoptarg, "hil") == 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
return print_usage("unrecognized flag");
|
||||
}
|
||||
}
|
||||
|
||||
// wait until task is up & running (the mode_* commands depend on it)
|
||||
if (wait_until_running() < 0) {
|
||||
_task_id = -1;
|
||||
PWMSim *instance = new PWMSim(hil_mode);
|
||||
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
PWMSim *PWMSim::instantiate(int argc, char *argv[])
|
||||
{
|
||||
return new PWMSim();
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
instance->ScheduleNow();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PWMSim::custom_command(int argc, char *argv[])
|
||||
{
|
||||
const char *verb = argv[0];
|
||||
|
||||
/* start the task if not running */
|
||||
if (!is_running()) {
|
||||
int ret = PWMSim::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
Mode servo_mode = MODE_NONE;
|
||||
|
||||
if (!strcmp(verb, "mode_pwm")) {
|
||||
servo_mode = PWMSim::MODE_8PWM;
|
||||
|
||||
} else if (!strcmp(verb, "mode_pwm16")) {
|
||||
servo_mode = PWMSim::MODE_16PWM;
|
||||
}
|
||||
|
||||
/* was a new mode set? */
|
||||
if (servo_mode != MODE_NONE) {
|
||||
/* switch modes */
|
||||
PWMSim *object = get_instance();
|
||||
|
||||
if (servo_mode != object->get_mode()) {
|
||||
/* (re)set the PWM output mode */
|
||||
return object->set_mode(servo_mode);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
@ -645,13 +291,8 @@ It is used in SITL and HITL.
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("pwm_out_sim", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task in mode_pwm16");
|
||||
|
||||
PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the pwm sim if not running already");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "use 8 PWM outputs");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm16", "use 16 PWM outputs");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('m', "sim", "hil|sim", "Mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
@ -659,30 +300,10 @@ It is used in SITL and HITL.
|
||||
|
||||
int PWMSim::print_status()
|
||||
{
|
||||
PX4_INFO("Running max update rate: %i Hz", _current_update_rate);
|
||||
PX4_INFO("Polling %i actuator controls", _poll_fds_num);
|
||||
|
||||
const char *mode_str = nullptr;
|
||||
|
||||
switch (_mode) {
|
||||
case MODE_8PWM: mode_str = "pwm8"; break;
|
||||
case MODE_16PWM: mode_str = "pwm16"; break;
|
||||
|
||||
case MODE_NONE: mode_str = "no pwm"; break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (mode_str) {
|
||||
PX4_INFO("PWM Mode: %s", mode_str);
|
||||
}
|
||||
|
||||
_mixing_output.printStatus();
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]);
|
||||
|
||||
int
|
||||
pwm_out_sim_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
@ -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();
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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] {};
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user