yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

mixer_module: create MixingOutput library and use in fmu

Browse Source 
This should be a pure refactoring, no functional change.
This commit is contained in:
Beat Küng
2019-08-22 08:22:57 +02:00
committed by Daniel Agar
parent 0ec6e79a0a
commit d3fb610fde
9 changed files with 831 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp
View File

@@ -57,6 +57,15 @@ public:
virtual void Run() = 0; virtual void Run() = 0;
/**
* Switch to a different WorkQueue.
* NOTE: Caller is responsible for synchronization.
*
* @param config The WorkQueue configuration (see WorkQueueManager.hpp).
* @return true if initialization was successful
*/
bool ChangeWorkQeue(const wq_config_t &config) { return Init(config); }
protected: protected:
/** /**

1
src/drivers/px4fmu/CMakeLists.txt
View File

@@ -40,5 +40,6 @@ px4_add_module(
arch_io_pins arch_io_pins
circuit_breaker circuit_breaker
mixer mixer
mixer_module
pwm_limit pwm_limit
) )

558
src/drivers/px4fmu/fmu.cpp
View File

@@ -50,7 +50,7 @@
#include <lib/cdev/CDev.hpp> #include <lib/cdev/CDev.hpp>
#include <lib/circuit_breaker/circuit_breaker.h> #include <lib/circuit_breaker/circuit_breaker.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/mixer/mixer.h> #include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h> #include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/pwm_limit/pwm_limit.h> #include <lib/pwm_limit/pwm_limit.h>
@@ -97,7 +97,8 @@ enum PortMode {
#define PX4FMU_DEVICE_PATH "/dev/px4fmu" #define PX4FMU_DEVICE_PATH "/dev/px4fmu"
class PX4FMU : public cdev::CDev, public ModuleBase<PX4FMU>, public px4::ScheduledWorkItem
class PX4FMU : public cdev::CDev, public ModuleBase<PX4FMU>, public OutputModuleInterface
{ {
public: public:
enum Mode { enum Mode {
@@ -161,14 +162,11 @@ public:
void update_pwm_trims(); void update_pwm_trims();
void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
private: private:
MixingOutput _mixing_output{*this, true};
enum class MotorOrdering : int32_t {
PX4 = 0,
Betaflight = 1
};
hrt_abstime _time_last_mix = 0;
static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
@@ -180,67 +178,25 @@ private:
unsigned _current_update_rate{0}; unsigned _current_update_rate{0};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _param_sub{ORB_ID(parameter_update)}; uORB::Subscription _param_sub{ORB_ID(parameter_update)};
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
unsigned _num_outputs{0}; unsigned _num_outputs{0};
int _class_instance{-1}; int _class_instance{-1};
bool _throttle_armed{false};
bool _pwm_on{false}; bool _pwm_on{false};
uint32_t _pwm_mask{0}; uint32_t _pwm_mask{0};
bool _pwm_initialized{false}; bool _pwm_initialized{false};
bool _test_mode{false}; bool _test_mode{false};
MixerGroup *_mixers{nullptr};
uint32_t _groups_required{0};
uint32_t _groups_subscribed{0};
bool _wq_hpdefault{true};
uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {
{this, ORB_ID(actuator_controls_0)},
{this, ORB_ID(actuator_controls_1)},
{this, ORB_ID(actuator_controls_2)},
{this, ORB_ID(actuator_controls_3)}
};
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
static pwm_limit_t _pwm_limit;
static actuator_armed_s _armed;
uint16_t _failsafe_pwm[MAX_ACTUATORS] {};
uint16_t _disarmed_pwm[MAX_ACTUATORS] {};
uint16_t _min_pwm[MAX_ACTUATORS] {};
uint16_t _max_pwm[MAX_ACTUATORS] {};
uint16_t _reverse_pwm_mask{0};
unsigned _num_disarmed_set{0}; unsigned _num_disarmed_set{0};
float _mot_t_max{0.0f}; ///< maximum rise time for motor (slew rate limiting)
float _thr_mdl_fac{0.0f}; ///< thrust to pwm modelling factor
Mixer::Airmode _airmode{Mixer::Airmode::disabled}; ///< multicopter air-mode
MotorOrdering _motor_ordering{MotorOrdering::PX4};
perf_counter_t _cycle_perf; perf_counter_t _cycle_perf;
perf_counter_t _cycle_interval_perf; perf_counter_t _cycle_interval_perf;
perf_counter_t _control_latency_perf;
static bool arm_nothrottle()
{
return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode);
}
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
void capture_callback(uint32_t chan_index, void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
void subscribe(); void update_current_rate();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg); int pwm_ioctl(file *filp, int cmd, unsigned long arg);
void update_pwm_rev_mask(); void update_pwm_rev_mask();
@@ -256,35 +212,17 @@ private:
PX4FMU(const PX4FMU &) = delete; PX4FMU(const PX4FMU &) = delete;
PX4FMU operator=(const PX4FMU &) = delete; PX4FMU operator=(const PX4FMU &) = delete;
/**
* Reorder PWM outputs according to _motor_ordering
* @param values PWM values to reorder
*/
inline void reorder_outputs(uint16_t values[MAX_ACTUATORS]);
}; };
pwm_limit_t PX4FMU::_pwm_limit;
actuator_armed_s PX4FMU::_armed = {};
PX4FMU::PX4FMU() : PX4FMU::PX4FMU() :
CDev(PX4FMU_DEVICE_PATH), CDev(PX4FMU_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default), OutputModuleInterface(px4::wq_configurations::hp_default),
_cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")), _cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")),
_cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval")), _cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval"))
_control_latency_perf(perf_alloc(PC_ELAPSED, "px4fmu: control latency"))
{ {
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { _mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
_min_pwm[i] = PWM_DEFAULT_MIN; _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
_max_pwm[i] = PWM_DEFAULT_MAX;
}
// Safely initialize armed flags.
_armed.armed = false;
_armed.prearmed = false;
_armed.ready_to_arm = false;
_armed.lockdown = false;
_armed.force_failsafe = false;
_armed.in_esc_calibration_mode = false;
} }
PX4FMU::~PX4FMU() PX4FMU::~PX4FMU()
@@ -297,7 +235,6 @@ PX4FMU::~PX4FMU()
perf_free(_cycle_perf); perf_free(_cycle_perf);
perf_free(_cycle_interval_perf); perf_free(_cycle_interval_perf);
perf_free(_control_latency_perf);
} }
int int
@@ -324,9 +261,6 @@ PX4FMU::init()
/* force a reset of the update rate */ /* force a reset of the update rate */
_current_update_rate = 0; _current_update_rate = 0;
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
// Getting initial parameter values // Getting initial parameter values
update_params(); update_params();
@@ -634,32 +568,8 @@ PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz)
} }
void void
PX4FMU::subscribe() PX4FMU::update_current_rate()
{ {
// must be locked to potentially change WorkQueue
lock();
// first clear everything
ScheduleClear();
for (auto &control_sub : _control_subs) {
control_sub.unregister_callback();
}
// if subscribed to control group 0 or 1 then move to the rate_ctrl WQ
const bool sub_group_0 = (_groups_required & (1 << 0));
const bool sub_group_1 = (_groups_required & (1 << 1));
if (_wq_hpdefault && (sub_group_0 || sub_group_1)) {
if (WorkItem::Init(px4::wq_configurations::rate_ctrl)) {
// let the new WQ handle the subscribe update
_wq_hpdefault = false;
ScheduleNow();
unlock();
return;
}
}
/* /*
* Adjust actuator topic update rate to keep up with * Adjust actuator topic update rate to keep up with
* the highest servo update rate configured. * the highest servo update rate configured.
@@ -675,33 +585,16 @@ PX4FMU::subscribe()
// max interval 0.5 - 100 ms (10 - 2000Hz) // max interval 0.5 - 100 ms (10 - 2000Hz)
const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000); const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000);
_current_update_rate = max_rate; _current_update_rate = max_rate;
_groups_subscribed = _groups_required; _mixing_output.setMaxTopicUpdateRate(update_interval_in_us);
// subscribe to all required actuator control groups with max interval set
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_required & (1 << i)) {
PX4_DEBUG("subscribe to actuator_controls_%d", i);
if (!_control_subs[i].register_callback()) {
PX4_ERR("actuator_controls_%d register callback failed!", i);
}
// limit subscription interval
_control_subs[i].set_interval_us(update_interval_in_us);
}
}
PX4_DEBUG("_groups_required 0x%08x", _groups_required);
PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed);
unlock();
} }
void void
PX4FMU::update_pwm_rev_mask() PX4FMU::update_pwm_rev_mask()
{ {
_reverse_pwm_mask = 0; uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
reverse_pwm_mask = 0;
const char *pname_format; const char *pname_format;
@@ -726,7 +619,7 @@ PX4FMU::update_pwm_rev_mask()
if (param_h != PARAM_INVALID) { if (param_h != PARAM_INVALID) {
int32_t ival = 0; int32_t ival = 0;
param_get(param_h, &ival); param_get(param_h, &ival);
_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
} }
} }
} }
@@ -736,42 +629,43 @@ PX4FMU::update_pwm_trims()
{ {
PX4_DEBUG("update_pwm_trims"); PX4_DEBUG("update_pwm_trims");
if (_mixers != nullptr) { if (!_mixing_output.mixers()) {
return;
int16_t values[MAX_ACTUATORS] = {};
const char *pname_format;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
pname_format = "PWM_MAIN_TRIM%d";
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
pname_format = "PWM_AUX_TRIM%d";
} else {
PX4_ERR("PWM TRIM only for MAIN and AUX");
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
char pname[16];
/* fill the struct from parameters */
sprintf(pname, pname_format, i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
float pval = 0.0f;
param_get(param_h, &pval);
values[i] = (int16_t)(10000 * pval);
PX4_DEBUG("%s: %d", pname, values[i]);
}
}
/* copy the trim values to the mixer offsets */
unsigned n_out = _mixers->set_trims(values, MAX_ACTUATORS);
PX4_DEBUG("set %d trims", n_out);
} }
int16_t values[MAX_ACTUATORS] = {};
const char *pname_format;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
pname_format = "PWM_MAIN_TRIM%d";
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
pname_format = "PWM_AUX_TRIM%d";
} else {
PX4_ERR("PWM TRIM only for MAIN and AUX");
return;
}
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
char pname[16];
/* fill the struct from parameters */
sprintf(pname, pname_format, i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
float pval = 0.0f;
param_get(param_h, &pval);
values[i] = (int16_t)(10000 * pval);
PX4_DEBUG("%s: %d", pname, values[i]);
}
}
/* copy the trim values to the mixer offsets */
unsigned n_out = _mixing_output.mixers()->set_trims(values, MAX_ACTUATORS);
PX4_DEBUG("set %d trims", n_out);
} }
int int
@@ -825,15 +719,35 @@ PX4FMU::update_pwm_out_state(bool on)
up_pwm_servo_arm(on); up_pwm_servo_arm(on);
} }
void PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
if (_test_mode) {
return;
}
/* output to the servos */
if (_pwm_initialized) {
for (size_t i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, outputs[i]);
}
}
/* Trigger all timer's channels in Oneshot mode to fire
* the oneshots with updated values.
*/
if (num_control_groups_updated > 0) {
up_pwm_update();
}
}
void void
PX4FMU::Run() PX4FMU::Run()
{ {
if (should_exit()) { if (should_exit()) {
ScheduleClear(); ScheduleClear();
_mixing_output.unregister();
for (auto &control_sub : _control_subs) {
control_sub.unregister_callback();
}
exit_and_cleanup(); exit_and_cleanup();
return; return;
@@ -842,133 +756,10 @@ PX4FMU::Run()
perf_begin(_cycle_perf); perf_begin(_cycle_perf);
perf_count(_cycle_interval_perf); perf_count(_cycle_interval_perf);
// check arming state _mixing_output.update();
if (_armed_sub.update(&_armed)) {
/* Update the armed status and check that we're not locked down.
* We also need to arm throttle for the ESC calibration. */
_throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode;
}
unsigned n_updates = 0;
if (_mixers != nullptr) {
/* get controls for required topics */
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_subscribed & (1 << i)) {
if (_control_subs[i].copy(&_controls[i])) {
n_updates++;
}
/* During ESC calibration, we overwrite the throttle value. */
if (i == 0 && _armed.in_esc_calibration_mode) {
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON;
}
}
}
if (_mot_t_max > FLT_EPSILON) {
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _time_last_mix) / 1e6f, 0.0001f, 0.02f);
_time_last_mix = now;
// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
_mixers->set_max_delta_out_once(delta_out_max);
}
if (_thr_mdl_fac > FLT_EPSILON) {
_mixers->set_thrust_factor(_thr_mdl_fac);
}
/* do mixing */
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs);
/* the PWM limit call takes care of out of band errors, NaN and constrains */
uint16_t pwm_limited[MAX_ACTUATORS] {};
pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask,
_disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
pwm_limited[i] = _failsafe_pwm[i];
}
}
/* overwrite outputs in case of lockdown or parachute triggering with disarmed PWM values */
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
pwm_limited[i] = _disarmed_pwm[i];
}
}
/* apply _motor_ordering */
reorder_outputs(pwm_limited);
/* output to the servos */
if (_pwm_initialized && !_test_mode) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
}
/* Trigger all timer's channels in Oneshot mode to fire
* the oneshots with updated values.
*/
if (n_updates > 0 && !_test_mode) {
up_pwm_update();
}
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = mixed_num_outputs;
// zero unused outputs
for (size_t i = 0; i < mixed_num_outputs; ++i) {
actuator_outputs.output[i] = pwm_limited[i];
}
actuator_outputs.timestamp = hrt_absolute_time();
_outputs_pub.publish(actuator_outputs);
/* 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;
_to_mixer_status.publish(motor_limits);
}
_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);
const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
if (required && (timestamp_sample > 0)) {
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
break;
}
}
}
/* update PWM status if armed or if disarmed PWM values are set */ /* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = _armed.armed || (_num_disarmed_set > 0) || _armed.in_esc_calibration_mode; bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode;
if (_pwm_on != pwm_on) { if (_pwm_on != pwm_on) {
_pwm_on = pwm_on; _pwm_on = pwm_on;
@@ -979,11 +770,13 @@ PX4FMU::Run()
update_params(); update_params();
} }
// check at end of cycle (subscribe() can potentially change to a different WorkQueue thread) if (_current_update_rate == 0) {
if ((_groups_subscribed != _groups_required) || (_current_update_rate == 0)) { update_current_rate();
subscribe();
} }
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
_mixing_output.updateSubscriptions(true);
perf_end(_cycle_perf); perf_end(_cycle_perf);
} }
@@ -995,78 +788,10 @@ void PX4FMU::update_params()
update_pwm_rev_mask(); update_pwm_rev_mask();
update_pwm_trims(); update_pwm_trims();
param_t param_handle; updateParams();
// maximum motor slew rate parameter
param_handle = param_find("MOT_SLEW_MAX");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, &_mot_t_max);
}
// thrust to pwm modelling factor
param_handle = param_find("THR_MDL_FAC");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, &_thr_mdl_fac);
}
// multicopter air-mode
param_handle = param_find("MC_AIRMODE");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, &_airmode);
}
// motor ordering
param_handle = param_find("MOT_ORDERING");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, (int32_t *)&_motor_ordering);
}
} }
int
PX4FMU::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];
/* limit control input */
if (input > 1.0f) {
input = 1.0f;
} else if (input < -1.0f) {
input = -1.0f;
}
/* motor spinup phase - lock throttle to zero */
if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
input = 0.0f;
}
}
/* throttle not arming - mark throttle input as invalid */
if (arm_nothrottle() && !_armed.in_esc_calibration_mode) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* set the throttle to an invalid value */
input = NAN;
}
}
return 0;
}
int int
PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
{ {
@@ -1178,21 +903,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (pwm->values[i] == 0) { if (pwm->values[i] == 0) {
/* ignore 0 */ /* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) { } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX; _mixing_output.failsafeValue(i) = PWM_HIGHEST_MAX;
} }
#if PWM_LOWEST_MIN > 0 #if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) { else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN; _mixing_output.failsafeValue(i) = PWM_LOWEST_MIN;
} }
#endif #endif
else { else {
_failsafe_pwm[i] = pwm->values[i]; _mixing_output.failsafeValue(i) = pwm->values[i];
} }
} }
@@ -1203,7 +928,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
pwm->values[i] = _failsafe_pwm[i]; pwm->values[i] = _mixing_output.failsafeValue(i);
} }
pwm->channel_count = MAX_ACTUATORS; pwm->channel_count = MAX_ACTUATORS;
@@ -1223,19 +948,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (pwm->values[i] == 0) { if (pwm->values[i] == 0) {
/* ignore 0 */ /* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) { } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX; _mixing_output.disarmedValue(i) = PWM_HIGHEST_MAX;
} }
#if PWM_LOWEST_MIN > 0 #if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) { else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN; _mixing_output.disarmedValue(i) = PWM_LOWEST_MIN;
} }
#endif #endif
else { else {
_disarmed_pwm[i] = pwm->values[i]; _mixing_output.disarmedValue(i) = pwm->values[i];
} }
} }
@@ -1246,7 +971,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
_num_disarmed_set = 0; _num_disarmed_set = 0;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
if (_disarmed_pwm[i] > 0) { if (_mixing_output.disarmedValue(i) > 0) {
_num_disarmed_set++; _num_disarmed_set++;
} }
} }
@@ -1258,7 +983,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
pwm->values[i] = _disarmed_pwm[i]; pwm->values[i] = _mixing_output.disarmedValue(i);
} }
pwm->channel_count = MAX_ACTUATORS; pwm->channel_count = MAX_ACTUATORS;
@@ -1278,20 +1003,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (pwm->values[i] == 0) { if (pwm->values[i] == 0) {
/* ignore 0 */ /* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) { } else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN; _mixing_output.minValue(i) = PWM_HIGHEST_MIN;
} }
#if PWM_LOWEST_MIN > 0 #if PWM_LOWEST_MIN > 0
else if (pwm->values[i] < PWM_LOWEST_MIN) { else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN; _mixing_output.minValue(i) = PWM_LOWEST_MIN;
} }
#endif #endif
else { else {
_min_pwm[i] = pwm->values[i]; _mixing_output.minValue(i) = pwm->values[i];
} }
} }
@@ -1302,7 +1027,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
pwm->values[i] = _min_pwm[i]; pwm->values[i] = _mixing_output.minValue(i);
} }
pwm->channel_count = MAX_ACTUATORS; pwm->channel_count = MAX_ACTUATORS;
@@ -1323,13 +1048,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (pwm->values[i] == 0) { if (pwm->values[i] == 0) {
/* ignore 0 */ /* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) { } else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX; _mixing_output.maxValue(i) = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) { } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX; _mixing_output.maxValue(i) = PWM_HIGHEST_MAX;
} else { } else {
_max_pwm[i] = pwm->values[i]; _mixing_output.maxValue(i) = pwm->values[i];
} }
} }
@@ -1340,7 +1065,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < MAX_ACTUATORS; i++) { for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
pwm->values[i] = _max_pwm[i]; pwm->values[i] = _mixing_output.maxValue(i);
} }
pwm->channel_count = MAX_ACTUATORS; pwm->channel_count = MAX_ACTUATORS;
@@ -1358,14 +1083,14 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break; break;
} }
if (_mixers == nullptr) { if (_mixing_output.mixers() == nullptr) {
PX4_ERR("error: no mixer loaded"); PX4_ERR("error: no mixer loaded");
ret = -EIO; ret = -EIO;
break; break;
} }
/* copy the trim values to the mixer offsets */ /* copy the trim values to the mixer offsets */
_mixers->set_trims((int16_t *)pwm->values, pwm->channel_count); _mixing_output.mixers()->set_trims((int16_t *)pwm->values, pwm->channel_count);
PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]); PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]);
break; break;
@@ -1374,13 +1099,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_GET_TRIM_PWM: { case PWM_SERVO_GET_TRIM_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
if (_mixers == nullptr) { if (_mixing_output.mixers() == nullptr) {
memset(pwm, 0, sizeof(pwm_output_values)); memset(pwm, 0, sizeof(pwm_output_values));
PX4_WARN("warning: trim values not valid - no mixer loaded"); PX4_WARN("warning: trim values not valid - no mixer loaded");
} else { } else {
pwm->channel_count = _mixers->get_trims((int16_t *)pwm->values); pwm->channel_count = _mixing_output.mixers()->get_trims((int16_t *)pwm->values);
} }
break; break;
@@ -1736,47 +1461,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
} }
case MIXERIOCRESET: case MIXERIOCRESET:
if (_mixers != nullptr) { _mixing_output.resetMixer();
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ScheduleNow(); // run to clear schedule and callbacks
}
break; break;
case MIXERIOCLOADBUF: { case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg; const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024); unsigned buflen = strnlen(buf, 1024);
ret = _mixing_output.loadMixer(buf, buflen);
if (_mixers == nullptr) { update_pwm_trims();
_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);
PX4_DEBUG("loaded mixers \n%s\n", buf);
update_pwm_trims();
}
}
ScheduleNow(); // run to clear schedule and callbacks
break; break;
} }
@@ -1791,35 +1484,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
return ret; return ret;
} }
void
PX4FMU::reorder_outputs(uint16_t values[MAX_ACTUATORS])
{
if (MAX_ACTUATORS < 4) {
return;
}
if (_motor_ordering == MotorOrdering::Betaflight) {
/*
* Betaflight default motor ordering:
* 4 2
* ^
* 3 1
*/
const uint16_t pwm_tmp[4] = {values[0], values[1], values[2], values[3] };
values[0] = pwm_tmp[3];
values[1] = pwm_tmp[0];
values[2] = pwm_tmp[1];
values[3] = pwm_tmp[2];
}
/* else: PX4, no need to reorder
* 3 1
* ^
* 2 4
*/
}
void void
PX4FMU::sensor_reset(int ms) PX4FMU::sensor_reset(int ms)
{ {
@@ -2639,7 +2303,7 @@ int PX4FMU::print_status()
perf_print_counter(_cycle_perf); perf_print_counter(_cycle_perf);
perf_print_counter(_cycle_interval_perf); perf_print_counter(_cycle_interval_perf);
perf_print_counter(_control_latency_perf); _mixing_output.printStatus();
return 0; return 0;
} }

1
src/lib/CMakeLists.txt
View File

@@ -51,6 +51,7 @@ add_subdirectory(landing_slope)
add_subdirectory(led) add_subdirectory(led)
add_subdirectory(mathlib) add_subdirectory(mathlib)
add_subdirectory(mixer) add_subdirectory(mixer)
add_subdirectory(mixer_module)
add_subdirectory(perf) add_subdirectory(perf)
add_subdirectory(pid) add_subdirectory(pid)
add_subdirectory(pwm_limit) add_subdirectory(pwm_limit)

35
src/lib/mixer_module/CMakeLists.txt Normal file
View File

@@ -0,0 +1,35 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(mixer_module mixer_module.cpp)

463
src/lib/mixer_module/mixer_module.cpp Normal file
View File

@@ -0,0 +1,463 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "mixer_module.hpp"
#include <lib/circuit_breaker/circuit_breaker.h>
#include <px4_log.h>
using namespace time_literals;
MixingOutput::MixingOutput(OutputModuleInterface &interface, bool support_esc_calibration, bool ramp_up)
: ModuleParams(&interface),
_control_subs{
{&interface, ORB_ID(actuator_controls_0)},
{&interface, ORB_ID(actuator_controls_1)},
{&interface, ORB_ID(actuator_controls_2)},
{&interface, ORB_ID(actuator_controls_3)}
},
_support_esc_calibration(support_esc_calibration),
_interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
{
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
_pwm_limit.ramp_up = ramp_up;
/* Safely initialize armed flags */
_armed.armed = false;
_armed.prearmed = false;
_armed.ready_to_arm = false;
_armed.lockdown = false;
_armed.force_failsafe = false;
_armed.in_esc_calibration_mode = false;
// If there is no safety button, disable it on init.
#ifndef GPIO_BTN_SAFETY
_safety_off = true;
#endif
px4_sem_init(&_lock, 0, 1);
}
MixingOutput::~MixingOutput()
{
perf_free(_control_latency_perf);
delete _mixers;
px4_sem_destroy(&_lock);
}
void MixingOutput::printStatus()
{
perf_print_counter(_control_latency_perf);
PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched);
}
void MixingOutput::updateParams()
{
ModuleParams::updateParams();
bool safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
if (safety_disabled) {
_safety_off = true;
}
// update mixer if we have one
if (_mixers) {
if (_param_mot_slew_max.get() <= FLT_EPSILON) {
_mixers->set_max_delta_out_once(0.f);
}
_mixers->set_thrust_factor(_param_thr_mdl_fac.get());
_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
}
}
bool MixingOutput::updateSubscriptions(bool allow_wq_switch)
{
if (_groups_subscribed == _groups_required) {
return false;
}
// must be locked to potentially change WorkQueue
lock();
// first clear everything
_interface.ScheduleClear();
unregister();
// if subscribed to control group 0 or 1 then move to the rate_ctrl WQ
const bool sub_group_0 = (_groups_required & (1 << 0));
const bool sub_group_1 = (_groups_required & (1 << 1));
if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) {
if (_interface.ChangeWorkQeue(px4::wq_configurations::rate_ctrl)) {
// let the new WQ handle the subscribe update
_wq_switched = true;
_interface.ScheduleNow();
unlock();
return false;
}
}
_groups_subscribed = _groups_required;
// subscribe to all required actuator control groups
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_required & (1 << i)) {
PX4_DEBUG("subscribe to actuator_controls_%d", i);
if (!_control_subs[i].register_callback()) {
PX4_ERR("actuator_controls_%d register callback failed!", i);
}
}
}
setMaxTopicUpdateRate(_max_topic_update_interval_us);
// if nothing required keep periodic schedule (so the module can update other things)
if (_groups_required == 0) {
// TODO: this might need to be configurable depending on the module
_interface.ScheduleOnInterval(100_ms);
}
PX4_DEBUG("_groups_required 0x%08x", _groups_required);
PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed);
unlock();
return true;
}
void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us)
{
_max_topic_update_interval_us = max_topic_update_interval_us;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_subscribed & (1 << i)) {
_control_subs[i].set_interval_us(_max_topic_update_interval_us);
}
}
}
void MixingOutput::setAllMinValues(uint16_t value)
{
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_min_value[i] = value;
}
}
void MixingOutput::setAllMaxValues(uint16_t value)
{
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_max_value[i] = value;
}
}
void MixingOutput::setAllFailsafeValues(uint16_t value)
{
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_failsafe_value[i] = value;
}
}
void MixingOutput::setAllDisarmedValues(uint16_t value)
{
for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
_disarmed_value[i] = value;
}
}
void MixingOutput::unregister()
{
for (auto &control_sub : _control_subs) {
control_sub.unregister_callback();
}
}
bool MixingOutput::update()
{
if (!_mixers) {
// do nothing until we have a valid mixer
return false;
}
// check arming state
if (_armed_sub.update(&_armed)) {
_armed.in_esc_calibration_mode &= _support_esc_calibration;
/* 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);
}
if (_param_mot_slew_max.get() > FLT_EPSILON) {
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _time_last_mix) / 1e6f, 0.0001f, 0.02f);
_time_last_mix = now;
// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float delta_out_max = 2.0f * 1000.0f * dt / (_max_value[0] - _min_value[0]) / _param_mot_slew_max.get();
_mixers->set_max_delta_out_once(delta_out_max);
}
unsigned n_updates = 0;
/* get controls for required topics */
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_groups_subscribed & (1 << i)) {
if (_control_subs[i].copy(&_controls[i])) {
n_updates++;
}
/* During ESC calibration, we overwrite the throttle value. */
if (i == 0 && _armed.in_esc_calibration_mode) {
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON;
}
}
}
/* do mixing */
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, MAX_ACTUATORS);
/* the PWM limit call takes care of out of band errors, NaN and constrains */
uint16_t output_limited[MAX_ACTUATORS] {};
pwm_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
_disarmed_value, _min_value, _max_value, outputs, output_limited, &_pwm_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
output_limited[i] = _failsafe_value[i];
}
}
/* overwrite outputs in case of lockdown or parachute triggering with disarmed values */
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
output_limited[i] = _disarmed_value[i];
}
}
bool stop_motors = true;
if (mixed_num_outputs > 0) {
/* assume if one (here the 1.) motor is disarmed, all of them should be stopped */
stop_motors = (output_limited[0] == _disarmed_value[0]);
}
/* apply _param_mot_ordering */
reorderOutputs(output_limited);
/* now return the outputs to the driver */
_interface.updateOutputs(stop_motors, output_limited, mixed_num_outputs, n_updates);
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = mixed_num_outputs;
// zero unused outputs
for (size_t i = 0; i < mixed_num_outputs; ++i) {
actuator_outputs.output[i] = output_limited[i];
}
actuator_outputs.timestamp = hrt_absolute_time();
_outputs_pub.publish(actuator_outputs);
/* 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 = actuator_outputs.timestamp;
motor_limits.saturation_status = saturation_status.value;
_to_mixer_status.publish(motor_limits);
}
// 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 &timestamp_sample = _controls[i].timestamp_sample;
if (required && (timestamp_sample > 0)) {
perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample);
break;
}
}
// check safety button state
if (_safety_sub.updated()) {
safety_s safety;
if (_safety_sub.copy(&safety)) {
_safety_off = !safety.safety_switch_available || safety.safety_off;
}
}
return true;
}
void
MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
{
if (MAX_ACTUATORS < 4) {
return;
}
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
/*
* Betaflight default motor ordering:
* 4 2
* ^
* 3 1
*/
const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] };
values[0] = value_tmp[3];
values[1] = value_tmp[0];
values[2] = value_tmp[1];
values[3] = value_tmp[2];
}
/* else: PX4, no need to reorder
* 3 1
* ^
* 2 4
*/
}
int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
{
const MixingOutput *output = (const MixingOutput *)handle;
input = output->_controls[control_group].control[control_index];
/* limit control input */
if (input > 1.0f) {
input = 1.0f;
} else if (input < -1.0f) {
input = -1.0f;
}
/* motor spinup phase - lock throttle to zero */
if (output->_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
input = 0.0f;
}
}
/* throttle not arming - mark throttle input as invalid */
if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* set the throttle to an invalid value */
input = NAN;
}
}
return 0;
}
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);
}
if (_mixers == nullptr) {
_groups_required = 0;
return -ENOMEM;
}
int ret = _mixers->load_from_buf(buf, len);
if (ret != 0) {
PX4_ERR("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
return ret;
}
_mixers->groups_required(_groups_required);
PX4_DEBUG("loaded mixers \n%s\n", buf);
updateParams();
lock();
_interface.ScheduleNow();
unlock();
return ret;
}

203
src/lib/mixer_module/mixer_module.hpp Normal file
View File

@@ -0,0 +1,203 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <board_config.h>
#include <lib/mixer/mixer.h>
#include <lib/perf/perf_counter.h>
#include <lib/pwm_limit/pwm_limit.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
/**
* @class OutputModuleInterface
* Base class for an output module.
*/
class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams
{
public:
static constexpr int MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
OutputModuleInterface(const px4::wq_config_t &config)
: px4::ScheduledWorkItem(config), ModuleParams(nullptr) {}
virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) = 0;
};
/**
* @class MixingOutput
* This handles the mixing, arming/disarming and all subscriptions required for that.
*
* It also drives the scheduling of the OutputModuleInterface (via uORB callbacks
* to reduce output latency).
*/
class MixingOutput : public ModuleParams
{
public:
static constexpr int MAX_ACTUATORS = OutputModuleInterface::MAX_ACTUATORS;
/**
* Contructor
* @param interface Parent module for scheduling, parameter updates and callbacks
* @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, bool support_esc_calibration, bool ramp_up = true);
~MixingOutput();
void printStatus();
/**
* Call this regularly from Run(). It will call interface.updateOutputs().
* @return true if outputs were updated
*/
bool update();
/**
* Check for subscription updates (e.g. after a mixer is loaded).
* Call this at the very end of Run() if allow_wq_switch
* @param allow_wq_switch if true
* @return true if subscriptions got changed
*/
bool updateSubscriptions(bool allow_wq_switch);
/**
* unregister uORB subscription callbacks
*/
void unregister();
void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us);
void resetMixer();
int loadMixer(const char *buf, unsigned len);
const actuator_armed_s &armed() const { return _armed; }
MixerGroup *mixers() const { return _mixers; }
void setAllFailsafeValues(uint16_t value);
void setAllDisarmedValues(uint16_t value);
void setAllMinValues(uint16_t value);
void setAllMaxValues(uint16_t value);
uint16_t &reverseOutputMask() { return _reverse_output_mask; }
uint16_t &failsafeValue(int index) { return _failsafe_value[index]; }
/** Disarmed values: if ramp_up is true, then disarmedValue < minValue needs to hold */
uint16_t &disarmedValue(int index) { return _disarmed_value[index]; }
uint16_t &minValue(int index) { return _min_value[index]; }
uint16_t &maxValue(int index) { return _max_value[index]; }
protected:
void updateParams() override;
private:
bool armNoThrottle() const
{
return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode;
}
static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
enum class MotorOrdering : int32_t {
PX4 = 0,
Betaflight = 1
};
/**
* Reorder PWM outputs according to _param_mot_ordering
* @param values PWM values to reorder
*/
inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]);
void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
void unlock() { px4_sem_post(&_lock); }
px4_sem_t _lock; /**< lock to protect access to work queue changes (includes ScheduleNow calls from another thread) */
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 _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction
pwm_limit_t _pwm_limit;
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
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
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)
bool _safety_off{false}; ///< State of the safety button from the subscribed _safety_sub topic
bool _throttle_armed{false};
MixerGroup *_mixers{nullptr};
uint32_t _groups_required{0};
uint32_t _groups_subscribed{0};
const bool _support_esc_calibration;
bool _wq_switched{false};
OutputModuleInterface &_interface;
perf_counter_t _control_latency_perf;
DEFINE_PARAMETERS(
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode, ///< multicopter air-mode
(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to pwm modelling factor
(ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering
)
};

8
src/lib/pwm_limit/pwm_limit.cpp
View File

@@ -54,6 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit)
{ {
limit->state = PWM_LIMIT_STATE_INIT; limit->state = PWM_LIMIT_STATE_INIT;
limit->time_armed = 0; limit->time_armed = 0;
limit->ramp_up = true;
} }
void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
@@ -81,7 +82,12 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
case PWM_LIMIT_STATE_OFF: case PWM_LIMIT_STATE_OFF:
if (armed) { if (armed) {
limit->state = PWM_LIMIT_STATE_RAMP; if (limit->ramp_up) {
limit->state = PWM_LIMIT_STATE_RAMP;
} else {
limit->state = PWM_LIMIT_STATE_ON;
}
/* reset arming time, used for ramp timing */ /* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time(); limit->time_armed = hrt_absolute_time();

1
src/lib/pwm_limit/pwm_limit.h
View File

@@ -67,6 +67,7 @@ enum pwm_limit_state {
typedef struct { typedef struct {
enum pwm_limit_state state; enum pwm_limit_state state;
uint64_t time_armed; uint64_t time_armed;
bool ramp_up; ///< if true, motors will ramp up from disarmed to min_pwm after arming
} pwm_limit_t; } pwm_limit_t;
__EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_init(pwm_limit_t *limit);