diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 72633bee59..3a787521a1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1178,9 +1178,8 @@ PX4FMU::cycle() _mixers->set_max_delta_out_once(delta_out_max); } - if (_thr_mdl_fac > FLT_EPSILON) { - _mixers->set_thrust_factor(_thr_mdl_fac); - } + _mixers->set_thrust_factor(_thr_mdl_fac); + _mixers->set_airmode(_airmode); /* do mixing */ float outputs[_max_actuators]; @@ -1246,8 +1245,6 @@ PX4FMU::cycle() orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance, ORB_PRIO_DEFAULT); } - _mixers->set_airmode(_airmode); - // 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); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 4d480ca50b..e43ada2421 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -425,6 +425,12 @@ void UavcanNode::update_params() if (param_handle != PARAM_INVALID) { param_get(param_handle, &_airmode); } + + param_handle = param_find("THR_MDL_FAC"); + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &_thr_mdl_factor); + } } int UavcanNode::start_fw_server() @@ -905,6 +911,7 @@ int UavcanNode::run() // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; + _mixers->set_thrust_factor(_thr_mdl_factor); _mixers->set_airmode(_airmode); // Do mixing diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 13d4606e75..f84b85cf5b 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -211,6 +211,7 @@ private: perf_counter_t _perf_control_latency; Mixer::Airmode _airmode = Mixer::Airmode::disabled; + float _thr_mdl_factor = 0.0f; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 99113ad5b3..5aa632421e 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -128,6 +128,7 @@ #pragma once #include +#include /** simple channel scaler */ struct mixer_scaler_s { @@ -707,7 +708,10 @@ public: * * @param[in] val The value */ - void set_thrust_factor(float val) override { _thrust_factor = val; } + void set_thrust_factor(float val) override + { + _thrust_factor = math::constrain(val, 0.0f, 1.0f); + } void set_airmode(Airmode airmode) override;