C++ MixerGroup类代码示例

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;
}

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);
		}


//.........这里部分代码省略.........

int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	int ret = OK;
	int channel;

	switch (cmd) {
	case PWM_SERVO_ARM:
		up_pwm_servo_arm(true);
		break;

	case PWM_SERVO_DISARM:
		up_pwm_servo_arm(false);
		break;

	case PWM_SERVO_SET(2):
	case PWM_SERVO_SET(3):
		if (_mode != MODE_4PWM) {
			ret = -EINVAL;
			break;
		}

		/* FALLTHROUGH */
	case PWM_SERVO_SET(0):
	case PWM_SERVO_SET(1):
		if (arg < 2100) {
			channel = cmd - PWM_SERVO_SET(0);
			up_pwm_servo_set(channel, arg);

		} else {
			ret = -EINVAL;
		}

		break;

	case PWM_SERVO_GET(2):
	case PWM_SERVO_GET(3):
		if (_mode != MODE_4PWM) {
			ret = -EINVAL;
			break;
		}

		/* FALLTHROUGH */
	case PWM_SERVO_GET(0):
	case PWM_SERVO_GET(1): {
			channel = cmd - PWM_SERVO_SET(0);
			*(servo_position_t *)arg = up_pwm_servo_get(channel);
			break;
		}

	case MIXERIOCGETOUTPUTCOUNT:
		if (_mode == MODE_4PWM) {
			*(unsigned *)arg = 4;

		} else {
			*(unsigned *)arg = 2;
		}

		break;

	case MIXERIOCRESET:
		if (_mixers != nullptr) {
			delete _mixers;
			_mixers = nullptr;
		}

		break;

	case MIXERIOCADDSIMPLE: {
			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;

			SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);

			if (mixer->check()) {
				delete mixer;
				ret = -EINVAL;

			} else {
				if (_mixers == nullptr)
					_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);

				_mixers->add_mixer(mixer);
			}

			break;
		}

	case MIXERIOCADDMULTIROTOR:
		/* XXX not yet supported */
		ret = -ENOTTY;
		break;

	case MIXERIOCLOADFILE: {
			const char *path = (const char *)arg;

			if (_mixers != nullptr) {
				delete _mixers;
				_mixers = nullptr;
			}

//.........这里部分代码省略.........

//.........这里部分代码省略.........
			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)
{

	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_mix_threadsafe(float *outputs, volatile uint16_t *limits)
{
	/* poor mans mutex */
	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) {
		return 0;
	}

	in_mixer = true;
	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
	*limits = mixer_group.get_saturation_status();
	in_mixer = false;

	return mixcount;
}

void
mixer_set_failsafe()
{
	/*
	 * Check if a custom failsafe value has been written,
	 * or if the mixer is not ok and bail out.
	 */

	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
	    !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
		return;
	}

	/* set failsafe defaults to the values for all inputs = 0 */
	float	outputs[PX4IO_SERVO_COUNT];
	unsigned mixed;

	if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
		// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
		// factor 2 is needed because actuator ouputs are in the range [-1,1]
		float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
					      r_setup_slew_max);
		mixer_group.set_max_delta_out_once(delta_out_max);
	}

	/* update parameter for mc thrust model if it updated */
	if (update_mc_thrust_param) {
		mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
		update_mc_thrust_param = false;
	}

	/* mix */

	mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);

	/* scale to PWM and update the servo outputs as required */
	for (unsigned i = 0; i < mixed; i++) {

		/* scale to servo output */
		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;

	}

	/* disable the rest of the outputs */
	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
		r_page_servo_failsafe[i] = 0;
	}

}

static void
mixer_set_failsafe()
{
	/* 
	 * Check if a custom failsafe value has been written,
	 * or if the mixer is not ok and bail out.
	 */

	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
		return;

	/* set failsafe defaults to the values for all inputs = 0 */
	float	outputs[PX4IO_SERVO_COUNT];
	unsigned mixed;

	/* mix */
	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);

	/* scale to PWM and update the servo outputs as required */
	for (unsigned i = 0; i < mixed; i++) {

		/* scale to servo output */
		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;

	}

	/* disable the rest of the outputs */
	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
		r_page_servo_failsafe[i] = 0;

}

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;
}

/**
 * Main loop function
 */
void
PCA9685::i2cpwm()
{
	if (_mode == IOX_MODE_TEST_OUT) {
		setPin(0, PCA9685_PWMCENTER);
		_should_run = true;

	} else if (_mode == IOX_MODE_OFF) {
		_should_run = false;

	} else {
		if (!_mode_on_initialized) {
			// Init PWM limits
			pwm_limit_init(&_pwm_limit);

			// Get arming state
			_armed_sub = orb_subscribe(ORB_ID(actuator_armed));

			/* Subscribe to actuator groups */
			subscribe();

			/* set the uorb update interval lower than the driver pwm interval */
			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) {
				orb_set_interval(_control_subs[i], 1000.0f / PCA9685_PWMFREQ - 5);
			}

			_mode_on_initialized = true;
		}

		/* check if anything updated */
		int ret = ::poll(_poll_fds, _poll_fds_num, 0);

		if (ret < 0) {
			DEVICE_LOG("poll error %d", errno);

		} else if (ret == 0) {
	//			warnx("no PWM: failsafe");
		} else {
			/* get controls for required topics */
			unsigned poll_id = 0;

			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					if (_poll_fds[poll_id].revents & POLLIN) {
						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
					}
				}
				poll_id++;
			}

			if (_mixers != nullptr) {
				size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;

				// do mixing
				num_outputs = _mixers->mix(_outputs, num_outputs, NULL);

				// 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) {
						_outputs[i] = NAN_VALUE;
					}
				}

				// Finally, write servo values to motors
				for (int i = 0; i < num_outputs; i++) {
					uint16_t new_value = PCA9685_PWMCENTER +
								 (_outputs[i] * M_PI_F * PCA9685_SCALE);
					// DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
					//      (double)_controls[1].control[i]);

					if (isfinite(new_value) &&
						new_value >= PCA9685_PWMMIN &&
						new_value <= PCA9685_PWMMAX) {

						setPin(i, new_value);
						_rates[i] = new_value;
					}
				}
			}
		}

		bool updated;

		// Update Arming state
		orb_check(_armed_sub, &updated);
		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);

			bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;

			if (_servo_armed != set_armed) {
				_servo_armed = set_armed;
			}
		}

		// Update AUX controls update
		// orb_check(_actuator_controls_sub, &updated);
//.........这里部分代码省略.........

void
mixer_tick(void)
{

    /* check that we are receiving fresh data from the FMU */
    if ((system_state.fmu_data_received_time == 0) ||
            hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {

        /* too long without FMU input, time to go to failsafe */
        if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
            isr_debug(1, "AP RX timeout");
        }
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
        r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;

    } else {
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;

        /* this flag is never cleared once OK */
        r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
    }

    /* default to failsafe mixing - it will be forced below if flag is set */
    source = MIX_FAILSAFE;

    /*
     * Decide which set of controls we're using.
     */

    /* do not mix if RAW_PWM mode is on and FMU is good */
    if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
            (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

        /* don't actually mix anything - we already have raw PWM values */
        source = MIX_NONE;

    } else {

        if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
                (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
                (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {

            /* mix from FMU controls */
            source = MIX_FMU;
        }

        if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
                (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
                (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
                !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
                !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
                /* do not enter manual override if we asked for termination failsafe and FMU is lost */
                !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {

            /* if allowed, mix from RC inputs directly */
            source = MIX_OVERRIDE;
        } else 	if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
                     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
                     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
                     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
                     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

            /* if allowed, mix from RC inputs directly up to available rc channels */
            source = MIX_OVERRIDE_FMU_OK;
        }
    }

    /*
     * Decide whether the servos should be armed right now.
     *
     * We must be armed, and we must have a PWM source; either raw from
     * FMU or from the mixer.
     *
     */
    should_arm = (
                     /* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
                     /* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
                     /* and FMU is armed */ 		  && (
                         ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
                          /* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
                         /* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
                         /* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
                     )
                 );

    should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
                               && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
                               && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);

    /*
     * Check if failsafe termination is set - if yes,
     * set the force failsafe flag once entering the first
     * failsafe condition.
     */
    if (	/* if we have requested flight termination style failsafe (noreturn) */
        (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
        /* and we ended up in a failsafe condition */
        (source == MIX_FAILSAFE) &&
        /* and we should be armed, so we intended to provide outputs */
        should_arm &&
//.........这里部分代码省略.........

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;
}

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_tick(void)
{

	/* check that we are receiving fresh data from the FMU */
	if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {

		/* too long without FMU input, time to go to failsafe */
		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
			isr_debug(1, "AP RX timeout");
		}
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;

	} else {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
	}

	/* default to failsafe mixing */
	source = MIX_FAILSAFE;

	/*
	 * Decide which set of controls we're using.
	 */

	/* do not mix if RAW_PWM mode is on and FMU is good */
	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

		/* don't actually mix anything - we already have raw PWM values */
		source = MIX_NONE;

	} else {

		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {

			/* mix from FMU controls */
			source = MIX_FMU;
		}

		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
		     !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

		 	/* if allowed, mix from RC inputs directly */
			source = MIX_OVERRIDE;
		} else 	if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
		     (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

			/* if allowed, mix from RC inputs directly up to available rc channels */
			source = MIX_OVERRIDE_FMU_OK;
		}
	}

	/*
	 * Set failsafe status flag depending on mixing source
	 */
	if (source == MIX_FAILSAFE) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
	} else {
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
	}

	/*
	 * Decide whether the servos should be armed right now.
	 *
	 * We must be armed, and we must have a PWM source; either raw from
	 * FMU or from the mixer.
	 *
	 * XXX correct behaviour for failsafe may require an additional case
	 * here.
	 */
	should_arm = (
		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
		/* and FMU is armed */ 		  && (
							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
						     )
	);

	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);

	/*
	 * Run the mixers.
	 */
	if (source == MIX_FAILSAFE) {

		/* copy failsafe values to the servo outputs */
//.........这里部分代码省略.........

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 */
	PX4_ATOMIC_MODIFY_CLEAR(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)) {
			PX4_ATOMIC_MODIFY_CLEAR(r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
			return 0;
		}

		/* check if the last item has been processed - bail out if not */
		if (mixer_update_pending) {
			return 1;
		}

		/* 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);

		/* flag the buffer as ready */
		mixer_update_pending = true;

		break;
	}

	return 0;
}