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

px4fmu: publish mixer saturation data to uORB

Browse Source
This commit is contained in:
Paul Riseborough
2016-11-24 16:23:12 +11:00
committed by Lorenz Meier
parent ca6f67fd3b
commit da6d4398e9
1 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -87,7 +87,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/adc_report.h>
#include <uORB/topics/multirotor_motor_limits.h>
#ifdef HRT_PPM_CHANNEL
# include <systemlib/ppm_decode.h>
@@ -230,6 +230,7 @@ private:
bool _safety_off;
bool _safety_disabled;
orb_advert_t _to_safety;
orb_advert_t _to_mixer_status; ///< mixer status flags
float _mot_t_max; // maximum rise time for motor (slew rate limiting)
@@ -343,9 +344,9 @@ PX4FMU::PX4FMU() :
_safety_off(false),
_safety_disabled(false),
_to_safety(nullptr),
_to_mixer_status(nullptr),
_mot_t_max(0.0f),
_ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat"))
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -1166,6 +1167,18 @@ PX4FMU::cycle()
float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL);
/* publish mixer status */
multirotor_motor_limits_s multirotor_motor_limits = {};
multirotor_motor_limits.saturation_status = _mixers->get_saturation_status();
if (_to_mixer_status == nullptr) {
_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits);
} else {
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits);
}
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
if (i >= num_outputs) {