diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e37f457392..6bd6ae63b5 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1461,14 +1461,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } case MIXERIOCRESET: - _mixing_output.resetMixer(); + _mixing_output.resetMixerThreadSafe(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - ret = _mixing_output.loadMixer(buf, buflen); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); update_pwm_trims(); break; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index f253d3ef0f..9f2218c9bd 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -210,6 +210,7 @@ void MixingOutput::unregister() bool MixingOutput::update() { if (!_mixers) { + handleCommands(); // do nothing until we have a valid mixer return false; } @@ -338,6 +339,8 @@ bool MixingOutput::update() } } + handleCommands(); + return true; } @@ -410,24 +413,15 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 void MixingOutput::resetMixer() { - // TODO: thread-safe - - lock(); - if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; _groups_required = 0; } - - _interface.ScheduleNow(); - unlock(); } int MixingOutput::loadMixer(const char *buf, unsigned len) { - // TODO: thread-safe - if (_mixers == nullptr) { _mixers = new MixerGroup(controlCallback, (uintptr_t)this); } @@ -451,13 +445,81 @@ int MixingOutput::loadMixer(const char *buf, unsigned len) PX4_DEBUG("loaded mixers \n%s\n", buf); updateParams(); + return ret; +} + +void MixingOutput::handleCommands() +{ + if (_command.command.load() == Command::Type::None) { + return; + } + + switch (_command.command.load()) { + case Command::Type::loadMixer: + _command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length); + break; + + case Command::Type::resetMixer: + resetMixer(); + _command.result = 0; + break; + + default: + break; + } + + // mark as done + _command.command.store(Command::Type::None); +} + +void MixingOutput::resetMixerThreadSafe() +{ + if (_command.command.load() != Command::Type::None) { + // Cannot happen, because we expect only one other thread to call this. + // But as a safety precaution we return here. + PX4_ERR("Command not None"); + return; + } lock(); + _command.command.store(Command::Type::resetMixer); + _interface.ScheduleNow(); unlock(); - return ret; + // wait until processed + while (_command.command.load() != Command::Type::None) { + usleep(1000); + } + +} + +int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len) +{ + if (_command.command.load() != Command::Type::None) { + // Cannot happen, because we expect only one other thread to call this. + // But as a safety precaution we return here. + PX4_ERR("Command not None"); + return -1; + } + + lock(); + + _command.mixer_buf = buf; + _command.mixer_buf_length = len; + _command.command.store(Command::Type::loadMixer); + + _interface.ScheduleNow(); + + unlock(); + + // wait until processed + while (_command.command.load() != Command::Type::None) { + usleep(1000); + } + + return _command.result; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 027ffa5055..518c516e8b 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -112,8 +113,21 @@ public: void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us); + /** + * Reset (unload) the complete mixer, called from another thread. + * This is thread-safe, as long as only one other thread at a time calls this. + */ + void resetMixerThreadSafe(); + void resetMixer(); + /** + * Load (append) a new mixer from a buffer, called from another thread. + * This is thread-safe, as long as only one other thread at a time calls this. + * @return 0 on success, <0 error otherwise + */ + int loadMixerThreadSafe(const char *buf, unsigned len); + int loadMixer(const char *buf, unsigned len); const actuator_armed_s &armed() const { return _armed; } @@ -136,6 +150,8 @@ protected: void updateParams() override; private: + void handleCommands(); + bool armNoThrottle() const { return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode; @@ -147,6 +163,19 @@ private: Betaflight = 1 }; + struct Command { + enum class Type : int { + None, + resetMixer, + loadMixer + }; + px4::atomic command{Type::None}; + const char *mixer_buf; + unsigned mixer_buf_length; + int result; + }; + Command _command; ///< incoming commands (from another thread) + /** * Reorder PWM outputs according to _param_mot_ordering * @param values PWM values to reorder