diff options
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 32 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_multirotor.cpp | 10 |
2 files changed, 26 insertions, 16 deletions
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 6dfe4fbef..b98531c4d 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -91,7 +91,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; - } + } } unsigned @@ -132,26 +132,32 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* use the next character as a hint to decide which mixer class to construct */ switch (*p) { - case 'Z': - m = NullMixer::from_text(p, buflen); - break; - case 'M': - m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); - break; - case 'R': - m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); - break; - default: - /* it's probably junk or whitespace */ - break; + case 'Z': + m = NullMixer::from_text(p, buflen); + break; + + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen); + break; + + default: + /* it's probably junk or whitespace */ + break; } + if (m != nullptr) { add_mixer(m); ret = 0; + } else { /* skip whitespace or junk in the buffer */ buflen--; } } + return ret; } diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 233025b21..3dd6dfcf7 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -166,10 +166,12 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl debug("multirotor parse failed on '%s'", buf); return nullptr; } + if (used > (int)buflen) { debug("multirotor spec used %d of %u", used, buflen); return nullptr; } + buflen -= used; if (!strcmp(geomname, "4+")) { @@ -226,10 +228,12 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* keep roll, pitch and yaw control to 0 below min thrust */ if (thrust <= min_thrust) { output_factor = 0.0f; - /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + } else if (thrust < startpoint_full_control && thrust > min_thrust) { - output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust); - /* and then stay at full control */ + output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); + /* and then stay at full control */ + } else { output_factor = max_thrust; } |