PX4 Firmware
PX4 Autopilot Software http://px4.io
|
This handles the mixing, arming/disarming and all subscriptions required for that. More...
#include <mixer_module.hpp>
Classes | |
struct | Command |
struct | MotorTest |
Public Types | |
enum | SchedulingPolicy { SchedulingPolicy::Disabled, SchedulingPolicy::Auto } |
Public Member Functions | |
MixingOutput (uint8_t max_num_outputs, OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up=true) | |
Contructor. More... | |
~MixingOutput () | |
void | setDriverInstance (uint8_t instance) |
void | printStatus () const |
bool | update () |
Call this regularly from Run(). More... | |
bool | updateSubscriptions (bool allow_wq_switch) |
Check for subscription updates (e.g. More... | |
void | unregister () |
unregister uORB subscription callbacks More... | |
void | setMaxTopicUpdateRate (unsigned max_topic_update_interval_us) |
void | resetMixerThreadSafe () |
Reset (unload) the complete mixer, called from another thread. More... | |
void | resetMixer () |
int | loadMixerThreadSafe (const char *buf, unsigned len) |
Load (append) a new mixer from a buffer, called from another thread. More... | |
int | loadMixer (const char *buf, unsigned len) |
const actuator_armed_s & | armed () const |
MixerGroup * | mixers () const |
void | setAllFailsafeValues (uint16_t value) |
void | setAllDisarmedValues (uint16_t value) |
void | setAllMinValues (uint16_t value) |
void | setAllMaxValues (uint16_t value) |
uint16_t & | reverseOutputMask () |
uint16_t & | failsafeValue (int index) |
uint16_t & | disarmedValue (int index) |
Disarmed values: disarmedValue < minValue needs to hold. More... | |
uint16_t & | minValue (int index) |
uint16_t & | maxValue (int index) |
int | reorderedMotorIndex (int index) const |
Get the motor index that maps from PX4 convention to the configured one. More... | |
void | setIgnoreLockdown (bool ignore_lockdown) |
Static Public Attributes | |
static constexpr int | MAX_ACTUATORS = OutputModuleInterface::MAX_ACTUATORS |
Protected Member Functions | |
void | updateParams () override |
Private Types | |
enum | MotorOrdering : int32_t { MotorOrdering::PX4 = 0, MotorOrdering::Betaflight = 1 } |
Private Member Functions | |
void | handleCommands () |
bool | armNoThrottle () const |
unsigned | motorTest () |
void | updateOutputSlewrate () |
void | setAndPublishActuatorOutputs (unsigned num_outputs, actuator_outputs_s &actuator_outputs) |
void | publishMixerStatus (const actuator_outputs_s &actuator_outputs) |
void | updateLatencyPerfCounter (const actuator_outputs_s &actuator_outputs) |
void | reorderOutputs (uint16_t values[MAX_ACTUATORS]) |
Reorder outputs according to _param_mot_ordering. More... | |
void | lock () |
void | unlock () |
Static Private Member Functions | |
static int | controlCallback (uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) |
Private Attributes | |
Command | _command |
incoming commands (from another thread) More... | |
px4_sem_t | _lock |
lock to protect access to work queue changes (includes ScheduleNow calls from another thread) More... | |
uint16_t | _failsafe_value [MAX_ACTUATORS] {} |
uint16_t | _disarmed_value [MAX_ACTUATORS] {} |
uint16_t | _min_value [MAX_ACTUATORS] {} |
uint16_t | _max_value [MAX_ACTUATORS] {} |
uint16_t | _current_output_value [MAX_ACTUATORS] {} |
current output values (reordered) More... | |
uint16_t | _reverse_output_mask {0} |
reverses the interval [min, max] -> [max, min], NOT motor direction More... | |
output_limit_t | _output_limit |
uORB::Subscription | _armed_sub {ORB_ID(actuator_armed)} |
uORB::SubscriptionCallbackWorkItem | _control_subs [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] |
uORB::PublicationMulti< actuator_outputs_s > | _outputs_pub {ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT} |
uORB::PublicationMulti< multirotor_motor_limits_s > | _to_mixer_status {ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT} |
mixer status flags More... | |
actuator_controls_s | _controls [actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {} |
actuator_armed_s | _armed {} |
hrt_abstime | _time_last_mix {0} |
unsigned | _max_topic_update_interval_us {0} |
max _control_subs topic update interval (0=unlimited) More... | |
bool | _throttle_armed {false} |
bool | _ignore_lockdown {false} |
if true, ignore the _armed.lockdown flag (for HIL outputs) More... | |
MixerGroup * | _mixers {nullptr} |
uint32_t | _groups_required {0} |
uint32_t | _groups_subscribed {1u << 31} |
initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS) More... | |
const SchedulingPolicy | _scheduling_policy |
const bool | _support_esc_calibration |
bool | _wq_switched {false} |
uint8_t | _driver_instance {0} |
for boards that supports multiple outputs (e.g. PX4IO + FMU) More... | |
const uint8_t | _max_num_outputs |
MotorTest | _motor_test |
OutputModuleInterface & | _interface |
perf_counter_t | _control_latency_perf |
This handles the mixing, arming/disarming and all subscriptions required for that.
It can also drive the scheduling of the OutputModuleInterface (via uORB callbacks to reduce output latency).
Definition at line 90 of file mixer_module.hpp.
|
strongprivate |
Enumerator | |
---|---|
PX4 | |
Betaflight |
Definition at line 200 of file mixer_module.hpp.
|
strong |
Enumerator | |
---|---|
Disabled | Do not drive scheduling (the module needs to call ScheduleOnInterval() for example) |
Auto | Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) |
Definition at line 95 of file mixer_module.hpp.
MixingOutput::MixingOutput | ( | uint8_t | max_num_outputs, |
OutputModuleInterface & | interface, | ||
SchedulingPolicy | scheduling_policy, | ||
bool | support_esc_calibration, | ||
bool | ramp_up = true |
||
) |
Contructor.
max_num_outputs | maximum number of supported outputs |
interface | Parent module for scheduling, parameter updates and callbacks |
scheduling_policy | |
support_esc_calibration | true if the output module supports ESC calibration via max, then min setting |
ramp_up | true if motor ramp up from disarmed to min upon arming is wanted |
Definition at line 44 of file mixer_module.cpp.
References _armed, _control_latency_perf, _interface, _lock, _max_num_outputs, _motor_test, _output_limit, _scheduling_policy, _support_esc_calibration, actuator_armed_s::armed, actuator_armed_s::force_failsafe, actuator_armed_s::in_esc_calibration_mode, actuator_armed_s::lockdown, MAX_ACTUATORS, ORB_ID, output_limit_init(), PC_ELAPSED, perf_alloc, actuator_armed_s::prearmed, output_limit_t::ramp_up, actuator_armed_s::ready_to_arm, uORB::Subscription::subscribe(), lps25h::test(), and MixingOutput::MotorTest::test_motor_sub.
MixingOutput::~MixingOutput | ( | ) |
Definition at line 80 of file mixer_module.cpp.
References _control_latency_perf, _lock, _mixers, and perf_free().
|
inline |
Definition at line 155 of file mixer_module.hpp.
References df_bebop_bus_wrapper::_armed.
Referenced by DShotOutput::Run(), PX4FMU::Run(), and UavcanNode::Run().
|
inlineprivate |
Definition at line 186 of file mixer_module.hpp.
References df_bebop_bus_wrapper::_armed, actuator_armed_s::armed, actuator_armed_s::in_esc_calibration_mode, and actuator_armed_s::prearmed.
Referenced by controlCallback(), and update().
|
staticprivate |
Definition at line 495 of file mixer_module.cpp.
References _armed, _controls, _output_limit, armNoThrottle(), actuator_controls_s::control, f(), actuator_armed_s::in_esc_calibration_mode, OUTPUT_LIMIT_STATE_RAMP, and output_limit_t::state.
Referenced by loadMixer().
|
inline |
Disarmed values: disarmedValue < minValue needs to hold.
Definition at line 167 of file mixer_module.hpp.
Referenced by PWMSim::ioctl(), and PX4FMU::pwm_ioctl().
|
inline |
Definition at line 165 of file mixer_module.hpp.
Referenced by PWMSim::ioctl(), and PX4FMU::pwm_ioctl().
|
private |
Definition at line 574 of file mixer_module.cpp.
References _command, MixingOutput::Command::command, loadMixer(), MixingOutput::Command::loadMixer, MixingOutput::Command::mixer_buf, MixingOutput::Command::mixer_buf_length, MixingOutput::Command::None, resetMixer(), MixingOutput::Command::resetMixer, and MixingOutput::Command::result.
Referenced by update().
int MixingOutput::loadMixer | ( | const char * | buf, |
unsigned | len | ||
) |
Definition at line 545 of file mixer_module.cpp.
References _groups_required, _interface, _mixers, controlCallback(), MixerGroup::groups_required(), MixerGroup::load_from_buf(), OutputModuleInterface::mixerChanged(), and updateParams().
Referenced by handleCommands().
int MixingOutput::loadMixerThreadSafe | ( | const char * | buf, |
unsigned | len | ||
) |
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.
Definition at line 622 of file mixer_module.cpp.
References _command, _interface, MixingOutput::Command::command, MixingOutput::Command::loadMixer, lock(), MixingOutput::Command::mixer_buf, MixingOutput::Command::mixer_buf_length, MixingOutput::Command::None, MixingOutput::Command::result, and unlock().
Referenced by PWMSim::ioctl(), UavcanNode::ioctl(), PX4FMU::pwm_ioctl(), and DShotOutput::pwm_ioctl().
|
inlineprivate |
Definition at line 224 of file mixer_module.hpp.
Referenced by loadMixerThreadSafe(), resetMixerThreadSafe(), and updateSubscriptions().
|
inline |
Definition at line 169 of file mixer_module.hpp.
Referenced by PWMSim::ioctl(), and PX4FMU::pwm_ioctl().
|
inline |
Definition at line 168 of file mixer_module.hpp.
Referenced by PWMSim::ioctl(), and PX4FMU::pwm_ioctl().
|
inline |
Definition at line 157 of file mixer_module.hpp.
References df_bebop_bus_wrapper::_mixers.
Referenced by PX4FMU::pwm_ioctl(), DShotOutput::Run(), PX4FMU::update_pwm_trims(), and DShotOutput::updateTelemetryNumMotors().
|
private |
Definition at line 234 of file mixer_module.cpp.
References _current_output_value, _disarmed_value, _driver_instance, _max_num_outputs, _max_value, _min_value, _motor_test, test_motor_s::action, test_motor_s::driver_instance, hrt_absolute_time(), hrt_elapsed_time(), MixingOutput::MotorTest::in_test_mode, MAX_ACTUATORS, test_motor_s::motor_number, reorderedMotorIndex(), MixingOutput::MotorTest::test_motor_sub, MixingOutput::MotorTest::timeout, test_motor_s::timeout_ms, test_motor_s::timestamp, uORB::Subscription::update(), and test_motor_s::value.
Referenced by update().
void MixingOutput::printStatus | ( | ) | const |
Definition at line 87 of file mixer_module.cpp.
References _control_latency_perf, _current_output_value, _disarmed_value, _driver_instance, _failsafe_value, _max_num_outputs, _max_value, _min_value, _mixers, _wq_switched, perf_print_counter(), and reorderedMotorIndex().
Referenced by UavcanNode::print_info(), PWMSim::print_status(), DShotOutput::print_status(), and PX4FMU::print_status().
|
private |
Definition at line 421 of file mixer_module.cpp.
References _mixers, _to_mixer_status, MultirotorMixer::saturation_status::flags, MixerGroup::get_saturation_status(), uORB::PublicationMulti< T >::publish(), multirotor_motor_limits_s::saturation_status, multirotor_motor_limits_s::timestamp, actuator_outputs_s::timestamp, MultirotorMixer::saturation_status::valid, and MultirotorMixer::saturation_status::value.
Referenced by update().
int MixingOutput::reorderedMotorIndex | ( | int | index | ) | const |
Get the motor index that maps from PX4 convention to the configured one.
index | motor index in [0, num_motors-1] |
Definition at line 478 of file mixer_module.cpp.
References Betaflight.
Referenced by motorTest(), printStatus(), DShotOutput::requestESCInfo(), DShotOutput::sendCommandThreadSafe(), and DShotOutput::updateOutputs().
|
inlineprivate |
Reorder outputs according to _param_mot_ordering.
values | values to reorder |
Definition at line 451 of file mixer_module.cpp.
References Betaflight.
Referenced by update().
void MixingOutput::resetMixer | ( | ) |
Definition at line 534 of file mixer_module.cpp.
References _groups_required, _interface, _mixers, and OutputModuleInterface::mixerChanged().
Referenced by handleCommands().
void MixingOutput::resetMixerThreadSafe | ( | ) |
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.
Definition at line 598 of file mixer_module.cpp.
References _command, _interface, MixingOutput::Command::command, lock(), MixingOutput::Command::None, MixingOutput::Command::resetMixer, and unlock().
Referenced by PWMSim::ioctl(), UavcanNode::ioctl(), PX4FMU::pwm_ioctl(), and DShotOutput::pwm_ioctl().
|
inline |
Definition at line 164 of file mixer_module.hpp.
Referenced by PX4FMU::update_pwm_rev_mask().
void MixingOutput::setAllDisarmedValues | ( | uint16_t | value | ) |
Definition at line 207 of file mixer_module.cpp.
References _disarmed_value, and MAX_ACTUATORS.
Referenced by DShotOutput::DShotOutput(), UavcanNode::init(), and PWMSim::PWMSim().
void MixingOutput::setAllFailsafeValues | ( | uint16_t | value | ) |
Definition at line 200 of file mixer_module.cpp.
References _failsafe_value, and MAX_ACTUATORS.
Referenced by PWMSim::PWMSim().
void MixingOutput::setAllMaxValues | ( | uint16_t | value | ) |
Definition at line 193 of file mixer_module.cpp.
References _max_value, and MAX_ACTUATORS.
Referenced by DShotOutput::DShotOutput(), UavcanNode::init(), PWMSim::PWMSim(), and PX4FMU::PX4FMU().
void MixingOutput::setAllMinValues | ( | uint16_t | value | ) |
Definition at line 186 of file mixer_module.cpp.
References _min_value, and MAX_ACTUATORS.
Referenced by DShotOutput::DShotOutput(), UavcanNode::enable_idle_throttle_when_armed(), UavcanNode::init(), PWMSim::PWMSim(), PX4FMU::PX4FMU(), and DShotOutput::update_params().
|
private |
Definition at line 408 of file mixer_module.cpp.
References _current_output_value, _outputs_pub, hrt_absolute_time(), actuator_outputs_s::noutputs, actuator_outputs_s::output, uORB::PublicationMulti< T >::publish(), and actuator_outputs_s::timestamp.
Referenced by update().
|
inline |
Definition at line 113 of file mixer_module.hpp.
References ll40ls::instance.
Referenced by DShotOutput::init(), and PX4FMU::init().
|
inline |
Definition at line 178 of file mixer_module.hpp.
Referenced by PWMSim::PWMSim().
void MixingOutput::setMaxTopicUpdateRate | ( | unsigned | max_topic_update_interval_us | ) |
Definition at line 175 of file mixer_module.cpp.
References _control_subs, _groups_subscribed, _max_topic_update_interval_us, and uORB::SubscriptionInterval::set_interval_us().
Referenced by UavcanNode::init(), PX4FMU::update_current_rate(), and updateSubscriptions().
|
inlineprivate |
Definition at line 225 of file mixer_module.hpp.
Referenced by loadMixerThreadSafe(), resetMixerThreadSafe(), and updateSubscriptions().
void MixingOutput::unregister | ( | ) |
unregister uORB subscription callbacks
Definition at line 214 of file mixer_module.cpp.
References _control_subs.
Referenced by PWMSim::Run(), DShotOutput::Run(), PX4FMU::Run(), UavcanNode::Run(), and updateSubscriptions().
bool MixingOutput::update | ( | ) |
Call this regularly from Run().
It will call interface.updateOutputs().
Definition at line 296 of file mixer_module.cpp.
References _armed, _armed_sub, _control_subs, _controls, _current_output_value, _disarmed_value, _failsafe_value, _groups_subscribed, _ignore_lockdown, _interface, _max_num_outputs, _max_value, _min_value, _mixers, _motor_test, _output_limit, _reverse_output_mask, _support_esc_calibration, _throttle_armed, actuator_armed_s::armed, armNoThrottle(), actuator_controls_s::control, FLT_EPSILON, actuator_armed_s::force_failsafe, handleCommands(), actuator_armed_s::in_esc_calibration_mode, MixingOutput::MotorTest::in_test_mode, actuator_armed_s::lockdown, actuator_armed_s::manual_lockdown, MAX_ACTUATORS, MixerGroup::mix(), motorTest(), output_limit_calc(), OUTPUT_LIMIT_STATE_ON, publishMixerStatus(), reorderOutputs(), setAndPublishActuatorOutputs(), output_limit_t::state, uORB::Subscription::update(), updateLatencyPerfCounter(), OutputModuleInterface::updateOutputs(), and updateOutputSlewrate().
Referenced by PWMSim::Run(), DShotOutput::Run(), and PX4FMU::Run().
|
private |
Definition at line 436 of file mixer_module.cpp.
References _control_latency_perf, _controls, _groups_required, hrt_abstime, perf_set_elapsed(), actuator_outputs_s::timestamp, and actuator_controls_s::timestamp_sample.
Referenced by update().
|
private |
Definition at line 221 of file mixer_module.cpp.
References _max_value, _min_value, _mixers, _time_last_mix, math::constrain(), dt, f(), hrt_absolute_time(), hrt_abstime, and MixerGroup::set_max_delta_out_once().
Referenced by update().
|
overrideprotected |
Definition at line 103 of file mixer_module.cpp.
References _mixers, f(), FLT_EPSILON, MixerGroup::set_airmode(), MixerGroup::set_max_delta_out_once(), and MixerGroup::set_thrust_factor().
Referenced by loadMixer().
bool MixingOutput::updateSubscriptions | ( | bool | allow_wq_switch | ) |
Check for subscription updates (e.g.
after a mixer is loaded). Call this at the very end of Run() if allow_wq_switch
allow_wq_switch | if true |
Definition at line 118 of file mixer_module.cpp.
References _control_subs, _groups_required, _groups_subscribed, _interface, _max_topic_update_interval_us, _scheduling_policy, _wq_switched, Auto, lock(), setMaxTopicUpdateRate(), unlock(), and unregister().
Referenced by PWMSim::Run(), DShotOutput::Run(), and PX4FMU::Run().
|
private |
Definition at line 244 of file mixer_module.hpp.
Referenced by controlCallback(), MixingOutput(), and update().
|
private |
Definition at line 237 of file mixer_module.hpp.
Referenced by update().
|
private |
incoming commands (from another thread)
Definition at line 216 of file mixer_module.hpp.
Referenced by handleCommands(), loadMixerThreadSafe(), and resetMixerThreadSafe().
|
private |
Definition at line 272 of file mixer_module.hpp.
Referenced by MixingOutput(), printStatus(), updateLatencyPerfCounter(), and ~MixingOutput().
|
private |
Definition at line 238 of file mixer_module.hpp.
Referenced by setMaxTopicUpdateRate(), unregister(), update(), and updateSubscriptions().
|
private |
Definition at line 243 of file mixer_module.hpp.
Referenced by controlCallback(), update(), and updateLatencyPerfCounter().
|
private |
current output values (reordered)
Definition at line 233 of file mixer_module.hpp.
Referenced by motorTest(), printStatus(), setAndPublishActuatorOutputs(), and update().
|
private |
Definition at line 230 of file mixer_module.hpp.
Referenced by motorTest(), printStatus(), setAllDisarmedValues(), and update().
|
private |
for boards that supports multiple outputs (e.g. PX4IO + FMU)
Definition at line 260 of file mixer_module.hpp.
Referenced by motorTest(), and printStatus().
|
private |
Definition at line 229 of file mixer_module.hpp.
Referenced by printStatus(), setAllFailsafeValues(), and update().
|
private |
Definition at line 253 of file mixer_module.hpp.
Referenced by loadMixer(), resetMixer(), updateLatencyPerfCounter(), and updateSubscriptions().
|
private |
initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS)
Definition at line 254 of file mixer_module.hpp.
Referenced by setMaxTopicUpdateRate(), update(), and updateSubscriptions().
|
private |
if true, ignore the _armed.lockdown flag (for HIL outputs)
Definition at line 250 of file mixer_module.hpp.
Referenced by update().
|
private |
Definition at line 270 of file mixer_module.hpp.
Referenced by loadMixer(), loadMixerThreadSafe(), MixingOutput(), resetMixer(), resetMixerThreadSafe(), update(), and updateSubscriptions().
|
private |
lock to protect access to work queue changes (includes ScheduleNow calls from another thread)
Definition at line 227 of file mixer_module.hpp.
Referenced by MixingOutput(), and ~MixingOutput().
|
private |
Definition at line 261 of file mixer_module.hpp.
Referenced by MixingOutput(), motorTest(), printStatus(), and update().
|
private |
max _control_subs topic update interval (0=unlimited)
Definition at line 247 of file mixer_module.hpp.
Referenced by setMaxTopicUpdateRate(), and updateSubscriptions().
|
private |
Definition at line 232 of file mixer_module.hpp.
Referenced by motorTest(), printStatus(), setAllMaxValues(), update(), and updateOutputSlewrate().
|
private |
Definition at line 231 of file mixer_module.hpp.
Referenced by motorTest(), printStatus(), setAllMinValues(), update(), and updateOutputSlewrate().
|
private |
Definition at line 252 of file mixer_module.hpp.
Referenced by loadMixer(), printStatus(), publishMixerStatus(), resetMixer(), update(), updateOutputSlewrate(), updateParams(), and ~MixingOutput().
|
private |
Definition at line 268 of file mixer_module.hpp.
Referenced by MixingOutput(), motorTest(), and update().
|
private |
Definition at line 235 of file mixer_module.hpp.
Referenced by controlCallback(), MixingOutput(), and update().
|
private |
Definition at line 240 of file mixer_module.hpp.
Referenced by setAndPublishActuatorOutputs().
|
private |
reverses the interval [min, max] -> [max, min], NOT motor direction
Definition at line 234 of file mixer_module.hpp.
Referenced by update().
|
private |
Definition at line 256 of file mixer_module.hpp.
Referenced by MixingOutput(), and updateSubscriptions().
|
private |
Definition at line 257 of file mixer_module.hpp.
Referenced by MixingOutput(), and update().
|
private |
Definition at line 249 of file mixer_module.hpp.
Referenced by update().
|
private |
Definition at line 246 of file mixer_module.hpp.
Referenced by updateOutputSlewrate().
|
private |
mixer status flags
Definition at line 241 of file mixer_module.hpp.
Referenced by publishMixerStatus().
|
private |
Definition at line 259 of file mixer_module.hpp.
Referenced by printStatus(), and updateSubscriptions().
|
static |
Definition at line 93 of file mixer_module.hpp.
Referenced by MixingOutput(), motorTest(), setAllDisarmedValues(), setAllFailsafeValues(), setAllMaxValues(), setAllMinValues(), and update().