int
TAP_ESC::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
switch (cmd) {
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr) {
_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) {
DEVICE_DEBUG("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
return ret;
}
void
mixer_handle_text(const void *buffer, size_t length)
{
px4io_mixdata *msg = (px4io_mixdata *)buffer;
debug("mixer text %u", length);
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
debug("reset");
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
/* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
return;
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
debug("buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
debug("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
}
break;
}
}
int
mixer_handle_text_create_mixer()
{
/* only run on update */
if (!mixer_update_pending) {
return 0;
}
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
/* abort if we're in the mixer - it will be tried again in the next iteration */
if (in_mixer) {
return 1;
}
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
/* enforce null termination */
mixer_text[resid] = '\0';
}
mixer_text_length = resid;
}
mixer_update_pending = false;
return 0;
}
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
/* disable mixing, will be enabled once load is complete */
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
/* abort if we're in the mixer - the caller is expected to retry */
if (in_mixer) {
return 1;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata)) {
return 0;
}
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* THEN actually delete it */
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return 0;
}
/* append mixer text and nul-terminate, guard against overflow */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
isr_debug(2, "buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
/* enforce null termination */
mixer_text[resid] = '\0';
}
mixer_text_length = resid;
}
break;
}
return 0;
}
int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
up_pwm_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate_channels;
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_SET(7):
case PWM_SERVO_SET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
break;
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
if (_mode < MODE_8PWM) {
ret = -EINVAL;
break;
}
#endif
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_mode < MODE_6PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
case PWM_SERVO_GET_RATEGROUP(4):
case PWM_SERVO_GET_RATEGROUP(5):
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case PWM_SERVO_GET_RATEGROUP(6):
case PWM_SERVO_GET_RATEGROUP(7):
#endif
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
case MODE_4PWM:
*(unsigned *)arg = 4;
break;
case MODE_2PWM:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
}
break;
case PWM_SERVO_SET_COUNT: {
/* change the number of outputs that are enabled for
* PWM. This is used to change the split between GPIO
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
*/
switch (arg) {
case 0:
set_mode(MODE_NONE);
break;
case 2:
set_mode(MODE_2PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
case 6:
set_mode(MODE_6PWM);
break;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
case 8:
set_mode(MODE_8PWM);
break;
#endif
default:
ret = -EINVAL;
break;
}
break;
}
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
}
break;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_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) {
debug("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
unlock();
return ret;
}
void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while outputs armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* FIRST mark the mixer as invalid */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* THEN actually delete it */
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
isr_debug(2, "buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
} else {
/* not yet reached the end of the mixer, set as not ok */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
}
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
/* update failsafe values */
mixer_set_failsafe();
}
break;
}
}
int
PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
switch (cmd) {
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
ret = OK;
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
_groups_required = 0;
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)_controls);
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);
ret = OK;
}
break;
}
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr) {
_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) {
DEVICE_DEBUG("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
ret = OK;
}
}
break;
}
case IOX_SET_MODE:
if (_mode != (IOX_MODE)arg) {
switch ((IOX_MODE)arg) {
case IOX_MODE_OFF:
warnx("shutting down");
break;
case IOX_MODE_ON:
warnx("starting");
break;
case IOX_MODE_TEST_OUT:
warnx("test starting");
break;
default:
return -1;
}
_mode = (IOX_MODE)arg;
}
// if not active, kick it
if (!_running) {
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
}
return OK;
default:
// see if the parent class can make any use of it
ret = CDev::ioctl(filp, cmd, arg);
break;
}
return ret;
}
bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loaded, unsigned expected_count,
const unsigned chunk_size,
bool verbose)
{
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
*/
/* load at once test */
unsigned xx = loaded;
mixer_group.reset();
mixer_group.load_from_buf(&buf[0], xx);
if (expected_count > 0) {
ut_compare("check number of mixers loaded", mixer_group.count(), expected_count);
}
unsigned empty_load = 2;
char empty_buf[2];
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
if (verbose) {
PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
}
ut_compare("empty buffer load", empty_load, 0);
/* reset, load in chunks */
mixer_group.reset();
char mixer_text[PX4IO_MAX_MIXER_LENGHT]; /* large enough for one mixer */
unsigned mixer_text_length = 0;
unsigned transmitted = 0;
unsigned resid = 0;
while (transmitted < loaded) {
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
PX4_ERR("Mixer text length overflow for file: %s. Is PX4IO_MAX_MIXER_LENGHT too small? (curr len: %d)", filename,
PX4IO_MAX_MIXER_LENGHT);
return false;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
//fprintf(stderr, "buflen %u, text:\n\"%s\"\n", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
//PX4_INFO("loaded %d mixers, used %u\n", mixer_group.count(), mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
/* enforce null termination */
mixer_text[resid] = '\0';
}
mixer_text_length = resid;
}
transmitted += text_length;
if (verbose) {
PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
}
}
if (verbose) {
PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
}
if (expected_count > 0 && mixer_group.count() != expected_count) {
PX4_ERR("Load of mixer failed, last chunk: %s, transmitted: %u, text length: %u, resid: %u", mixer_text, transmitted,
mixer_text_length, resid);
ut_compare("check number of mixers loaded (chunk)", mixer_group.count(), expected_count);
}
return true;
}
void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
return;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* FIRST mark the mixer as invalid */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* THEN actually delete it */
mixer_group.reset();
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */
/* XXX could add just what will fit & try to parse, then repeat... */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
isr_debug(2, "buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
}
break;
}
}
