From 1901edf13c4c677d06717cd9b0192bb7c5a75c02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 27 Nov 2021 16:43:41 +0100 Subject: [PATCH] actuator_motors.msg: add reversible flags & implement in mixer_module --- msg/actuator_motors.msg | 2 ++ src/drivers/dshot/DShot.cpp | 4 +++- src/lib/mixer_module/actuator_test.cpp | 8 ++++++-- src/lib/mixer_module/actuator_test.hpp | 4 +++- src/lib/mixer_module/functions.cpp | 3 +-- src/lib/mixer_module/functions.hpp | 18 ++++++++++++------ src/lib/mixer_module/mixer_module.cpp | 7 +++++-- src/lib/mixer_module/mixer_module.hpp | 11 ++++++++--- 8 files changed, 40 insertions(+), 17 deletions(-) diff --git a/msg/actuator_motors.msg b/msg/actuator_motors.msg index 9e09111309..bd7766f740 100644 --- a/msg/actuator_motors.msg +++ b/msg/actuator_motors.msg @@ -2,6 +2,8 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +uint16 reversible_flags # bitset which motors are configured to be reversible + uint8 NUM_CONTROLS = 8 float32[8] control # range: [-1, 1], where 1 means maximum positive thrust, # -1 maximum negative (if not supported by the output, <0 maps to NaN), diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 71ac5fda0f..ad2c3b6ce1 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -444,6 +444,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } else { int telemetry_index = 0; + const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); + for (int i = 0; i < (int)num_outputs; i++) { uint16_t output = outputs[i]; @@ -452,7 +454,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], // This is in terms of DShot values, code below is in terms of actuator_output // Direction 1) 48 is the slowest, 1047 is the fastest. // Direction 2) 1049 is the slowest, 2047 is the fastest. - if (_param_dshot_3d_enable.get()) { + if (_param_dshot_3d_enable.get() || (reversible_outputs & (1u << i))) { if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) { output = DSHOT_DISARM_VALUE; diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index dc7c2bc967..9c08d639bd 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -41,7 +41,7 @@ ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATO reset(); } -void ActuatorTest::update(int num_outputs, bool reversible_motors, float thrust_curve) +void ActuatorTest::update(int num_outputs, float thrust_curve) { const hrt_abstime now = hrt_absolute_time(); @@ -74,7 +74,11 @@ void ActuatorTest::update(int num_outputs, bool reversible_motors, float thrust_ // handle motors if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) { - FunctionMotors::updateValues(reversible_motors, thrust_curve, &value, 1); + actuator_motors_s motors; + motors.reversible_flags = 0; + _actuator_motors_sub.copy(&motors); + int motor_idx = actuator_test.function - (int)OutputFunction::Motor1; + FunctionMotors::updateValues(motors.reversible_flags >> motor_idx, thrust_curve, &value, 1); } _current_outputs[i] = value; diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp index d75174c540..71b73e0ad8 100644 --- a/src/lib/mixer_module/actuator_test.hpp +++ b/src/lib/mixer_module/actuator_test.hpp @@ -37,6 +37,7 @@ #include #include +#include #include static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch"); @@ -55,7 +56,7 @@ public: void reset(); - void update(int num_outputs, bool reversible_motors, float thrust_curve); + void update(int num_outputs, float thrust_curve); void overrideValues(float outputs[MAX_ACTUATORS], int num_outputs); @@ -64,6 +65,7 @@ public: private: uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; bool _in_test_mode{false}; hrt_abstime _next_timeout{0}; diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp index 2d9c20c1b4..4b70676e9f 100644 --- a/src/lib/mixer_module/functions.cpp +++ b/src/lib/mixer_module/functions.cpp @@ -35,7 +35,6 @@ FunctionMotors::FunctionMotors(const Context &context) : _topic(&context.work_item, ORB_ID(actuator_motors)), - _reversible_motors(context.reversible_motors), _thrust_factor(context.thrust_factor) { for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { @@ -45,7 +44,7 @@ FunctionMotors::FunctionMotors(const Context &context) void FunctionMotors::update() { if (_topic.update(&_data)) { - updateValues(_reversible_motors, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); + updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); } } diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 8394f570e4..7a2a6da5ee 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/src/lib/mixer_module/functions.hpp @@ -52,7 +52,6 @@ class FunctionProviderBase public: struct Context { px4::WorkItem &work_item; - bool reversible_motors; const float &thrust_factor; }; @@ -73,6 +72,11 @@ public: virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; } virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; } + + /** + * Check whether the output (motor) is configured to be reversible + */ + virtual bool reversible(OutputFunction func) const { return false; } }; /** @@ -124,15 +128,17 @@ public: bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } - static inline void updateValues(bool reversible, float thrust_factor, float *values, int num_values); + static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values); + + bool reversible(OutputFunction func) const override + { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); } private: uORB::SubscriptionCallbackWorkItem _topic; actuator_motors_s _data{}; - const bool _reversible_motors; const float &_thrust_factor; }; -void FunctionMotors::updateValues(bool reversible, float thrust_factor, float *values, int num_values) +void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) { if (thrust_factor > FLT_EPSILON) { for (int i = 0; i < num_values; ++i) { @@ -143,8 +149,8 @@ void FunctionMotors::updateValues(bool reversible, float thrust_factor, float *v } } - if (!reversible) { - for (int i = 0; i < num_values; ++i) { + for (int i = 0; i < num_values; ++i) { + if ((reversible & (1u << i)) == 0) { if (values[i] < -FLT_EPSILON) { values[i] = NAN; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 648fc5800a..c3e7a07102 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -385,7 +385,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li cleanupFunctions(); - const FunctionProviderBase::Context context{_interface, _reversible_motors, _param_thr_mdl_fac.reference()}; + const FunctionProviderBase::Context context{_interface, _param_thr_mdl_fac.reference()}; int provider_indexes[MAX_ACTUATORS] {}; int next_provider = 0; int subscription_callback_provider_index = INT_MAX; @@ -776,11 +776,12 @@ bool MixingOutput::updateDynamicMixer() } // check for actuator test - _actuator_test.update(_max_num_outputs, _reversible_motors, _param_thr_mdl_fac.get()); + _actuator_test.update(_max_num_outputs, _param_thr_mdl_fac.get()); // get output values float outputs[MAX_ACTUATORS]; bool all_disabled = true; + _reversible_mask = 0; for (int i = 0; i < _max_num_outputs; ++i) { if (_functions[i]) { @@ -793,6 +794,8 @@ bool MixingOutput::updateDynamicMixer() outputs[i] = NAN; } + _reversible_mask |= (uint32_t)_functions[i]->reversible(_function_assignment[i]) << i; + } else { outputs[i] = NAN; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 30a24fb838..61893e01fc 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -82,7 +82,7 @@ public: unsigned num_outputs, unsigned num_control_groups_updated) = 0; /** called whenever the mixer gets updated/reset */ - virtual void mixerChanged() {}; + virtual void mixerChanged() {} }; /** @@ -215,6 +215,12 @@ public: void setLowrateSchedulingInterval(hrt_abstime interval) { _lowrate_schedule_interval = interval; } + /** + * Get the bitmask of reversible outputs (motors only). + * This might change at any time (while disarmed), so output drivers requiring this should query this regularly. + */ + uint32_t reversibleOutputs() const { return _reversible_mask; } + protected: void updateParams() override; @@ -338,12 +344,11 @@ private: bool _need_function_update{true}; bool _use_dynamic_mixing{false}; ///< set to _param_sys_ctrl_alloc on init (avoid changing after startup) bool _has_backup_schedule{false}; - bool _reversible_motors = - false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors) const char *const _param_prefix; ParamHandles _param_handles[MAX_ACTUATORS]; hrt_abstime _lowrate_schedule_interval{300_ms}; ActuatorTest _actuator_test{_function_assignment}; + uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only) uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback