From 349469cf753d91c6da864d83bd8f3737e20121c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 15 Oct 2019 10:58:20 +0200 Subject: [PATCH] refactor pwm_out_sim: use mixer_module and run on work queue Tested with SITL + HITL --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- Tools/uorb_graph/create.py | 1 - src/drivers/drv_pwm_output.h | 7 +- src/drivers/dshot/dshot.cpp | 5 +- src/drivers/pwm_out_sim/CMakeLists.txt | 2 + src/drivers/pwm_out_sim/PWMSim.cpp | 535 ++++-------------------- src/drivers/pwm_out_sim/PWMSim.hpp | 88 +--- src/drivers/px4fmu/fmu.cpp | 48 +-- src/lib/mixer_module/mixer_module.cpp | 11 +- src/lib/mixer_module/mixer_module.hpp | 10 +- 10 files changed, 137 insertions(+), 572 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 82bde1889f..2a4764ecb6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -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 diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index cd11e56420..64f03133e3 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -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\]$'), ] diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 714461dae3..e39efa171d 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -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 diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 349967cc88..1b32267270 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -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]; diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt index be07621b6a..77a5de2543 100644 --- a/src/drivers/pwm_out_sim/CMakeLists.txt +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -37,4 +37,6 @@ px4_add_module( PWMSim.cpp DEPENDS mixer + mixer_module + output_limit ) diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 94809cea10..2f3ee931ba 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -33,326 +33,57 @@ #include "PWMSim.hpp" -#include #include +#include #include #include -#include -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[]) { diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 802f5a450c..0640896c66 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -33,112 +33,50 @@ #pragma once -#include - #include #include #include #include #include -#include +#include #include #include #include #include -#include -#include -#include -#include #include -class PWMSim : public cdev::CDev, public ModuleBase +class PWMSim : public cdev::CDev, public ModuleBase, 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 _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(); }; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 31dfa6bc29..90582eb142 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index fa11a46170..6ca7595f29 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -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] {}; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index f0dfac3991..31f04ceca0 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -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;