コード例 #1
0
ファイル: bbot.cpp プロジェクト: jonbinney/bbot
  HighSpeedPWM(unsigned int pwm_rate) :
    _pwm_rate(pwm_rate), _pwm_period(1.0f / (float) pwm_rate)
  {
    /* There's some sleeps in here that might not all be necessary... */

    printf("Resetting GPIO\n");
    gpioReset();

    usleep(500000);

    /* Magic number borrowed from src/drivers/px4fmu/fmu.cpp */
    printf("Initializing PWM\n");
    up_pwm_servo_init(0x3f);

    usleep(500000);

    /* Set PWM Rate for all outputs */
    printf("Setting PWM rate\n");
    if(up_pwm_servo_set_rate(_pwm_rate) != OK) {
      printf("set rate failed");
      return;
    }

    usleep(500000);

    printf("Arming PWM\n");
    up_pwm_servo_arm(true);

    usleep(500000);
  }
コード例 #2
0
ファイル: mixer.cpp プロジェクト: Tiktiki/Firmware
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 we have raw PWM and FMU is ok. */
	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
	    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {

		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) {
			/* a channel based override has been
			 * triggered, with FMU active */
			source = MIX_OVERRIDE_FMU_OK;

		} else {
			/* don't actually mix anything - copy values from r_page_direct_pwm */
			source = MIX_NONE;
			memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
		}

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

		else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {

			if (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;

			} else {
				/* if allowed, mix from RC inputs directly */
				source = MIX_OVERRIDE;
			}
		}
	}

	/*
	 * 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_arm_nothrottle = (
					/* 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 there is valid input via or mixer */         && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_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 &&
		/* and FMU is initialized */
		(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
		r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
	}

	/*
	 * Check if we should force failsafe - and do it if we have to
	 */
	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
		source = MIX_FAILSAFE;
	}

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

	/*
	 * Set simple mixer trim values
	 * (there should be a "dirty" flag to indicate that r_page_servo_control_trim has changed)
	 */
	mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);


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

		/* copy failsafe values to the servo outputs */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			r_page_servos[i] = r_page_servo_failsafe[i];

			/* safe actuators for FMU feedback */
			r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
		}


	} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)
		   && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {

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

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

		/* the pwm limit call takes care of out of band errors */
		pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
			       r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);

		/* clamp unused outputs to zero */
		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
			r_page_servos[i] = 0;
			outputs[i] = 0.0f;
		}

		/* store normalized outputs */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
		}
	}

	/* set arming */
	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);

	/* lockdown means to send a valid pulse which disables the outputs */
	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
		needs_to_arm = true;
	}

	if (needs_to_arm && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;
		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
		isr_debug(5, "> PWM enabled");

	} else if (!needs_to_arm && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
		isr_debug(5, "> PWM disabled");
	}

	if (mixer_servos_armed && (should_arm || should_arm_nothrottle)
	    && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
		/* update the servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			up_pwm_servo_set(i, r_page_servos[i]);
		}

		/* set S.BUS1 or S.BUS2 outputs */

		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
			sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);

		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
			sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
		}

	} else if (mixer_servos_armed && (should_always_enable_pwm
					  || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
		/* set the disarmed servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
			/* copy values into reporting register */
			r_page_servos[i] = r_page_servo_disarmed[i];
		}

		/* set S.BUS1 or S.BUS2 outputs */
		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
			sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
		}

		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
			sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
		}
	}
}
コード例 #3
0
ファイル: drv_pwm_servo.c プロジェクト: Nox997/Firmware
void
up_pwm_servo_deinit(void)
{
	/* disable the timers */
	up_pwm_servo_arm(false);
}
コード例 #4
0
ファイル: fmu.cpp プロジェクト: Bjarne-Madsen/Firmware
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;
}
コード例 #5
0
ファイル: fmu.cpp プロジェクト: Bjarne-Madsen/Firmware
void
PX4FMU::task_main()
{
	/* force a reset of the update rate */
	_current_update_rate = 0;

	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_param_sub = orb_subscribe(ORB_ID(parameter_update));

	/* advertise the mixed control outputs */
	actuator_outputs_s outputs;
	memset(&outputs, 0, sizeof(outputs));

#ifdef HRT_PPM_CHANNEL
	// rc input, published to ORB
	struct rc_input_values rc_in;
	orb_advert_t to_input_rc = 0;

	memset(&rc_in, 0, sizeof(rc_in));
	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif

	/* initialize PWM limit lib */
	pwm_limit_init(&_pwm_limit);

	update_pwm_rev_mask();

	/* loop until killed */
	while (!_task_should_exit) {
		if (_groups_subscribed != _groups_required) {
			subscribe();
			_groups_subscribed = _groups_required;
			/* force setting update rate */
			_current_update_rate = 0;
		}

		/*
		 * Adjust actuator topic update rate to keep up with
		 * the highest servo update rate configured.
		 *
		 * We always mix at max rate; some channels may update slower.
		 */
		unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;

		if (_current_update_rate != max_rate) {
			_current_update_rate = max_rate;
			int update_rate_in_ms = int(1000 / _current_update_rate);

			/* reject faster than 500 Hz updates */
			if (update_rate_in_ms < 2) {
				update_rate_in_ms = 2;
			}

			/* reject slower than 10 Hz updates */
			if (update_rate_in_ms > 100) {
				update_rate_in_ms = 100;
			}

			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
				if (_control_subs[i] > 0) {
					orb_set_interval(_control_subs[i], update_rate_in_ms);
				}
			}

			// set to current max rate, even if we are actually checking slower/faster
			_current_update_rate = max_rate;
		}

		/* sleep waiting for data, stopping to check for PPM
		 * input at 50Hz */
		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);

		/* this would be bad... */
		if (ret < 0) {
			log("poll error %d", errno);
			continue;

		} else if (ret == 0) {
			/* timeout: no control data, switch to failsafe values */
//			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++;
				}
			}

			/* can we mix? */
			if (_mixers != nullptr) {

				unsigned num_outputs;

				switch (_mode) {
				case MODE_2PWM:
					num_outputs = 2;
					break;

				case MODE_4PWM:
					num_outputs = 4;
					break;

				case MODE_6PWM:
					num_outputs = 6;
					break;

				case MODE_8PWM:
					num_outputs = 8;
					break;
				default:
					num_outputs = 0;
					break;
				}

				/* do mixing */
				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
				outputs.timestamp = hrt_absolute_time();

				/* iterate actuators */
				for (unsigned i = 0; i < num_outputs; i++) {
					/* last resort: catch NaN and INF */
					if ((i >= outputs.noutputs) ||
						!isfinite(outputs.output[i])) {
						/*
						 * Value is NaN, INF or out of band - set to the minimum value.
						 * This will be clearly visible on the servo status and will limit the risk of accidentally
						 * spinning motors. It would be deadly in flight.
						 */
						outputs.output[i] = -1.0f;
					}
				}

				uint16_t pwm_limited[num_outputs];

				/* the PWM limit call takes care of out of band errors and constrains */
				pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);

				/* output to the servos */
				for (unsigned i = 0; i < num_outputs; i++) {
					up_pwm_servo_set(i, pwm_limited[i]);
				}

				/* publish mixed control outputs */
				if (_outputs_pub != nullptr) {
					_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
				} else {

					orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
				}
			}
		}

		/* check arming state */
		bool updated = false;
		orb_check(_armed_sub, &updated);

		if (updated) {
			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);

			/* update the armed status and check that we're not locked down */
			bool set_armed = _armed.armed && !_armed.lockdown;

			if (_servo_armed != set_armed)
				_servo_armed = set_armed;

			/* update PWM status if armed or if disarmed PWM values are set */
			bool pwm_on = (_armed.armed || _num_disarmed_set > 0);

			if (_pwm_on != pwm_on) {
				_pwm_on = pwm_on;
				up_pwm_servo_arm(pwm_on);
			}
		}

		orb_check(_param_sub, &updated);
		if (updated) {
			parameter_update_s pupdate;
			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);

			update_pwm_rev_mask();
		}

#ifdef HRT_PPM_CHANNEL

		// see if we have new PPM input data
		if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
			// we have a new PPM frame. Publish it.
			rc_in.channel_count = ppm_decoded_channels;

			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
			}

			for (uint8_t i = 0; i < rc_in.channel_count; i++) {
				rc_in.values[i] = ppm_buffer[i];
			}

			rc_in.timestamp_publication = ppm_last_valid_decode;
			rc_in.timestamp_last_signal = ppm_last_valid_decode;

			rc_in.rc_ppm_frame_length = ppm_frame_length;
			rc_in.rssi = RC_INPUT_RSSI_MAX;
			rc_in.rc_failsafe = false;
			rc_in.rc_lost = false;
			rc_in.rc_lost_frame_count = 0;
			rc_in.rc_total_frame_count = 0;

			/* lazily advertise on first publication */
			if (to_input_rc == 0) {
				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);

			} else {
				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
			}
		}

#endif

	}

	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
		if (_control_subs[i] > 0) {
			::close(_control_subs[i]);
			_control_subs[i] = -1;
		}
	}
	::close(_armed_sub);
	::close(_param_sub);

	/* make sure servos are off */
	up_pwm_servo_deinit();

	log("stopping");

	/* note - someone else is responsible for restoring the GPIO config */

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}
コード例 #6
0
ファイル: mixer.cpp プロジェクト: junhongs/PIXHAWK_Serial
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 &&
        /* and FMU is initialized */
        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
        r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
    }

    /*
     * Check if we should force failsafe - and do it if we have to
     */
    if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
        source = MIX_FAILSAFE;
    }

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

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

        /* copy failsafe values to the servo outputs */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_servos[i] = r_page_servo_failsafe[i];

            /* safe actuators for FMU feedback */
            r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
        }


    } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {

        float	outputs[PX4IO_SERVO_COUNT];
        unsigned mixed;

        /* mix */

        /* poor mans mutex */
        in_mixer = true;
        mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
        in_mixer = false;

        /* the pwm limit call takes care of out of band errors */
        pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);

        for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
            r_page_servos[i] = 0;

        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
        }
    }

    /* set arming */
    bool needs_to_arm = (should_arm || should_always_enable_pwm);

    /* check any conditions that prevent arming */
    if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
        needs_to_arm = false;
    }
    if (!should_arm && !should_always_enable_pwm) {
        needs_to_arm = false;
    }

    if (needs_to_arm && !mixer_servos_armed) {
        /* need to arm, but not armed */
        up_pwm_servo_arm(true);
        mixer_servos_armed = true;
        r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
        isr_debug(5, "> PWM enabled");

    } else if (!needs_to_arm && mixer_servos_armed) {
        /* armed but need to disarm */
        up_pwm_servo_arm(false);
        mixer_servos_armed = false;
        r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
        isr_debug(5, "> PWM disabled");
    }

    if (mixer_servos_armed && should_arm) {
        /* update the servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
            up_pwm_servo_set(i, r_page_servos[i]);
        }

        /* set S.BUS1 or S.BUS2 outputs */

        if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
            sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
        } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
            sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
        }

    } else if (mixer_servos_armed && should_always_enable_pwm) {
        /* set the disarmed servo outputs. */
        for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
            up_pwm_servo_set(i, r_page_servo_disarmed[i]);

        /* set S.BUS1 or S.BUS2 outputs */
        if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
            sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);

        if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
            sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
    }
}
コード例 #7
0
ファイル: mixer.cpp プロジェクト: avnishks/px4
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)) {

		 	/* if allowed, mix from RC inputs directly */
			source = MIX_OVERRIDE;
		}
	}

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

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

		/* copy failsafe values to the servo outputs */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			r_page_servos[i] = r_page_servo_failsafe[i];

			/* safe actuators for FMU feedback */
			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
		}


	} else if (source != MIX_NONE) {

		float	outputs[PX4IO_SERVO_COUNT];
		unsigned mixed;

		uint16_t ramp_promille;

		/* update esc init state, but only if we are truely armed and not just PWM enabled */
		if (mixer_servos_armed && should_arm) {

			switch (esc_state) {

				/* after arming, some ESCs need an initalization period, count the time from here */
				case ESC_OFF:
					esc_init_time = hrt_absolute_time();
					esc_state = ESC_INIT;
				break;

				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
				case ESC_INIT:
					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
						esc_state = ESC_RAMP;
					}
				break;

				/* then ramp until the min speed is reached */
				case ESC_RAMP:
					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
						esc_state = ESC_ON;
					}
				break;

				case ESC_ON:
				default:

				break;
			}
		} else {
			esc_state = ESC_OFF;
		}

		/* do the calculations during the ramp for all at once */
		if(esc_state == ESC_RAMP) {
			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
		}


		/* 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++) {

			/* save actuator values for FMU readback */
			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);

			switch (esc_state) {
				case ESC_INIT:
					r_page_servos[i] = (outputs[i] * 600 + 1500);
				break;

				case ESC_RAMP:
					r_page_servos[i] = (outputs[i]
					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
				break;

				case ESC_ON:
					r_page_servos[i] = (outputs[i]
					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
				break;

				case ESC_OFF:
				default:
				break;
			}
		}
		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
			r_page_servos[i] = 0;
	}

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

	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;
		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
		isr_debug(5, "> PWM enabled");

	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
		isr_debug(5, "> PWM disabled");

	}

	if (mixer_servos_armed && should_arm) {
		/* update the servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
			up_pwm_servo_set(i, r_page_servos[i]);

	} else if (mixer_servos_armed && should_always_enable_pwm) {
		/* set the idle servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
			up_pwm_servo_set(i, r_page_servo_idle[i]);
	}
}
コード例 #8
0
ファイル: mixer.cpp プロジェクト: DuinoPilot/Firmware
void
mixer_tick(void)
{
	bool should_arm;

	/*
	 * Decide which set of inputs we're using.
	 */
	if (system_state.mixer_use_fmu) {
		/* we have recent control data from the FMU */
		control_count = PX4IO_CONTROL_CHANNELS;
		control_values = &system_state.fmu_channel_data[0];

		/* check that we are receiving fresh data from the FMU */
		if (!system_state.fmu_data_received) {
			fmu_input_drops++;

			/* too many frames without FMU input, time to go to failsafe */
			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
				system_state.mixer_use_fmu = false;
			}

		} else {
			fmu_input_drops = 0;
			system_state.fmu_data_received = false;
		}

	} else if (system_state.rc_channels > 0) {
		/* we have control data from an R/C input */
		control_count = system_state.rc_channels;
		control_values = &system_state.rc_channel_data[0];

	} else {
		/* we have no control input */
		/* XXX builtin failsafe would activate here */
		control_count = 0;
	}

	/*
	 * Run the mixers if we have any control data at all.
	 */
	if (control_count > 0) {
		float	outputs[IO_SERVO_COUNT];
		unsigned mixed;

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

		/* scale to PWM and update the servo outputs as required */
		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
			if (i < mixed) {
				/* scale to servo output */
				system_state.servos[i] = (outputs[i] * 500.0f) + 1500;

			} else {
				/* set to zero to inhibit PWM pulse output */
				system_state.servos[i] = 0;
			}

			/*
			 * If we are armed, update the servo output.
			 */
			if (system_state.armed && system_state.arm_ok)
				up_pwm_servo_set(i, system_state.servos[i]);
		}
	}

	/*
	 * Decide whether the servos should be armed right now.
	 */
	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);

	if (should_arm && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;

	} else if (!should_arm && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
	}
}
コード例 #9
0
ファイル: mixer.cpp プロジェクト: HimanshuKamat/Firmware
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 | PX4IO_P_STATUS_FLAGS_RAW_PWM);
		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 mixer is invalid or 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_MIXER_OK)) {

		/* don't actually mix anything - we already have raw PWM values or
		 not a valid mixer. */
		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)) {

		 	/* if allowed, mix from RC inputs directly */
			source = MIX_OVERRIDE;
		}
	}

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

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

		/* copy failsafe values to the servo outputs */
		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
			r_page_servos[i] = r_page_servo_failsafe[i];

			/* safe actuators for FMU feedback */
			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
		}


	} else if (source != MIX_NONE) {

		float	outputs[IO_SERVO_COUNT];
		unsigned mixed;

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

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

			/* save actuator values for FMU readback */
			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);

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

		}
		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
			r_page_servos[i] = 0;
	}

	/*
	 * 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.
	 */
	bool should_arm = (
	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
		/* FMU is available or FMU is not available but override is an option */
		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
	);

	if (should_arm && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;

	} else if (!should_arm && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
	}

	if (mixer_servos_armed) {
		/* update the servo outputs. */
		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
			up_pwm_servo_set(i, r_page_servos[i]);
	}
}
コード例 #10
0
ファイル: fmu.cpp プロジェクト: tpetri/rgb_cube
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;
			}

			_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);

			if (_mixers->load_from_file(path) != 0) {
				delete _mixers;
				_mixers = nullptr;
				ret = -EINVAL;
			}

			break;
		}

	default:
		ret = -ENOTTY;
		break;
	}

	return ret;
}
コード例 #11
0
ファイル: fmu.cpp プロジェクト: tpetri/rgb_cube
void
FMUServo::task_main()
{
	/* configure for PWM output */
	switch (_mode) {
	case MODE_2PWM:
		/* multi-port with flow control lines as PWM */
		/* XXX magic numbers */
		up_pwm_servo_init(0x3);
		break;

	case MODE_4PWM:
		/* multi-port as 4 PWM outs */
		/* XXX magic numbers */
		up_pwm_servo_init(0xf);
		break;

	case MODE_NONE:
		/* we should never get here... */
		break;
	}

	/* subscribe to objects that we are interested in watching */
	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	orb_set_interval(_t_actuators, 20);		/* 50Hz update rate */

	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */

	struct pollfd fds[2];
	fds[0].fd = _t_actuators;
	fds[0].events = POLLIN;
	fds[1].fd = _t_armed;
	fds[1].events = POLLIN;

	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;

	log("starting");

	/* loop until killed */
	while (!_task_should_exit) {

		/* sleep waiting for data, but no more than 100ms */
		int ret = ::poll(&fds[0], 2, 1000);

		/* this would be bad... */
		if (ret < 0) {
			log("poll error %d", errno);
			usleep(1000000);
			continue;
		}

		/* do we have a control update? */
		if (fds[0].revents & POLLIN) {
			float outputs[num_outputs];

			/* get controls - must always do this to avoid spinning */
			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);

			/* can we mix? */
			if (_mixers != nullptr) {

				/* do mixing */
				_mixers->mix(&outputs[0], num_outputs);

				/* iterate actuators */
				for (unsigned i = 0; i < num_outputs; i++) {

					/* scale for PWM output 900 - 2100us */
					up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
				}
			}
		}

		/* how about an arming update? */
		if (fds[1].revents & POLLIN) {
			struct actuator_armed_s aa;

			/* get new value */
			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);

			/* update PMW servo armed status */
			up_pwm_servo_arm(aa.armed);
		}
	}

	::close(_t_actuators);
	::close(_t_armed);

	/* make sure servos are off */
	up_pwm_servo_deinit();

	log("stopping");

	/* note - someone else is responsible for restoring the GPIO config */

	/* tell the dtor that we are exiting */
	_task = -1;
	_exit(0);
}
コード例 #12
0
void
PX4FMU::cycle()
{
	if (!_initialized) {

		/* force a reset of the update rate */
		_current_update_rate = 0;

		_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
		//_param_sub = orb_subscribe(ORB_ID(parameter_update));

		/* initialize PWM limit lib */
		pwm_limit_init(&_pwm_limit);

		update_pwm_rev_mask();

#ifdef RC_SERIAL_PORT
		_sbus_fd = sbus_init(RC_SERIAL_PORT, true);
#endif
		_initialized = true;
	}

	if (_groups_subscribed != _groups_required) {
		subscribe();
		_groups_subscribed = _groups_required;
		/* force setting update rate */
		_current_update_rate = 0;
	}

	/*
	 * Adjust actuator topic update rate to keep up with
	 * the highest servo update rate configured.
	 *
	 * We always mix at max rate; some channels may update slower.
	 */
	unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;

	if (_current_update_rate != max_rate) {
		_current_update_rate = max_rate;
		int update_rate_in_ms = int(1000 / _current_update_rate);

		/* reject faster than 500 Hz updates */
		if (update_rate_in_ms < 2) {
			update_rate_in_ms = 2;
		}

		/* reject slower than 10 Hz updates */
		if (update_rate_in_ms > 100) {
			update_rate_in_ms = 100;
		}

		//DEVICE_DEBUG("adjusted actuator update interval to %ums\n", update_rate_in_ms);

		for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
			if (_control_subs[i] > 0) {
				orb_set_interval(_control_subs[i], update_rate_in_ms);
			}
		}

		// set to current max rate, even if we are actually checking slower/faster
		_current_update_rate = max_rate;
	}

	/* check if anything updated */
    //从mkblctrl-blctrl电子模块驱动拿数据,貌似没用到,而且poll里面也在等待定时器导致定时器卡死
	int ret = 0;//::poll(_poll_fds, _poll_fds_num, 0);

	/* this would be bad... */
	if (ret < 0) {
		DEVICE_LOG("poll error %d\n", ret);

	} else if (ret == 0) {
		/* timeout: no control data, switch to failsafe values */
//			warnx("no PWM: failsafe\n");

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

		/* can we mix? */
		if (_mixers != NULL) {

			size_t num_outputs;

			switch (_mode) {
			case MODE_2PWM:
				num_outputs = 2;
				break;

			case MODE_4PWM:
				num_outputs = 4;
				break;

			case MODE_6PWM:
				num_outputs = 6;
				break;

			case MODE_8PWM:
				num_outputs = 8;
				break;

			default:
				num_outputs = 0;
				break;
			}

			/* do mixing */
			float outputs[_max_actuators];
			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;
				}
			}

			uint16_t pwm_limited[_max_actuators];

			/* the PWM limit call takes care of out of band errors, NaN and constrains */
			pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm,
				       outputs, pwm_limited, &_pwm_limit);

			/* output to the servos */
			for (size_t i = 0; i < num_outputs; i++) {
				up_pwm_servo_set(i, pwm_limited[i]);
			}

			publish_pwm_outputs(pwm_limited, num_outputs);
		}
	}

	/* check arming state */
	bool updated = false;
	orb_check(_armed_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);

		/* update the armed status and check that we're not locked down */
		bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;

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

		/* update PWM status if armed or if disarmed PWM values are set */
		bool pwm_on = (set_armed || _num_disarmed_set > 0);

		if (_pwm_on != pwm_on) {
			_pwm_on = pwm_on;
			up_pwm_servo_arm(pwm_on);
		}
	}
/* TODO:F
	orb_check(_param_sub, &updated);

	if (updated) {
		parameter_update_s pupdate;
		orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);

		update_pwm_rev_mask();
	}
*/
	bool rc_updated = false;

#ifdef RC_SERIAL_PORT
	bool sbus_failsafe, sbus_frame_drop;
	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
	uint16_t raw_rc_count;
	bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
				       input_rc_s::RC_INPUT_MAX_CHANNELS);

	if (sbus_updated) {
		// we have a new PPM frame. Publish it.
		_rc_in.channel_count = raw_rc_count;

		if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
			_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
		}

		for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
			_rc_in.values[i] = raw_rc_values[i];
       //     pilot_info("value[%d]=%d\n", i, _rc_in.values[i]);
		}

		_rc_in.timestamp_publication = hrt_absolute_time();
		_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;

		_rc_in.rc_ppm_frame_length = 0;
		_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
		_rc_in.rc_failsafe = false;
		_rc_in.rc_lost = false;
		_rc_in.rc_lost_frame_count = 0;
		_rc_in.rc_total_frame_count = 0;

		rc_updated = true;
	}
#endif

#ifdef HRT_PPM_CHANNEL

	// see if we have new PPM input data
	if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) &&
		ppm_decoded_channels > 3) {
		// we have a new PPM frame. Publish it.
		_rc_in.channel_count = ppm_decoded_channels;

		if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
			_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
		}

		for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
			_rc_in.values[i] = ppm_buffer[i];
		}

		_rc_in.timestamp_publication = ppm_last_valid_decode;
		_rc_in.timestamp_last_signal = ppm_last_valid_decode;

		_rc_in.rc_ppm_frame_length = ppm_frame_length;
		_rc_in.rssi = RC_INPUT_RSSI_MAX;
		_rc_in.rc_failsafe = false;
		_rc_in.rc_lost = false;
		_rc_in.rc_lost_frame_count = 0;
		_rc_in.rc_total_frame_count = 0;
	
		rc_updated = true;
	}

#endif

	if (rc_updated) {
		/* lazily advertise on first publication */
		if (_to_input_rc == NULL) {
			_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);

		} else {
			orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);
		}
	}

//	xTimerStart(_work, (2/portTICK_PERIOD_MS));
}
コード例 #13
0
ファイル: mixer.cpp プロジェクト: smithandrewc/Firmware
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 */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			r_page_servos[i] = r_page_servo_failsafe[i];

			/* safe actuators for FMU feedback */
			r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
		}


	} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {

		float	outputs[PX4IO_SERVO_COUNT];
		unsigned mixed;

		/* mix */

		/* poor mans mutex */
		in_mixer = true;
		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
		in_mixer = false;

		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);

		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
			r_page_servos[i] = 0;

		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
		}
	}

	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;
		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
		isr_debug(5, "> PWM enabled");

	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
		isr_debug(5, "> PWM disabled");
	}

	if (mixer_servos_armed && should_arm) {
		/* update the servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
			up_pwm_servo_set(i, r_page_servos[i]);

	} else if (mixer_servos_armed && should_always_enable_pwm) {
		/* set the disarmed servo outputs. */
		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
	}
}
コード例 #14
0
ファイル: mixer.cpp プロジェクト: abisely/Firmware
void
mixer_tick(void)
{
	bool should_arm;

	/* check that we are receiving fresh data from the FMU */
	if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
		/* too many frames without FMU input, time to go to failsafe */
		system_state.mixer_manual_override = true;
		system_state.mixer_fmu_available = false;
		lib_lowprintf("RX timeout\n");
	}

	/*
	 * Decide which set of inputs we're using.
	 */
	 
	/* this is for planes, where manual override makes sense */
	if (system_state.manual_override_ok) {
		/* if everything is ok */
		if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
			/* we have recent control data from the FMU */
			control_count = PX4IO_CONTROL_CHANNELS;
			control_values = &system_state.fmu_channel_data[0];

		} else if (system_state.rc_channels > 0) {
			/* when override is on or the fmu is not available, but RC is present */
			control_count = system_state.rc_channels;

			sched_lock();

			/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
			rc_channel_data[ROLL]     = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
			rc_channel_data[PITCH]    = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
			rc_channel_data[YAW]      = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
			rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
			//rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1];

			/* get the remaining channels, no remapping needed */
			for (unsigned i = 4; i < system_state.rc_channels; i++) {
				rc_channel_data[i] = system_state.rc_channel_data[i];
			}

			/* scale the control inputs */ 
			rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / 
				(float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000;

			if (rc_channel_data[THROTTLE] > 2000) {
				rc_channel_data[THROTTLE] = 2000;
			}

			if (rc_channel_data[THROTTLE] < 1000) {
				rc_channel_data[THROTTLE] = 1000;
			}
			
			// lib_lowprintf("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
			// 	(int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]),
			// 	(int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE]));

			control_values = &rc_channel_data[0];
			sched_unlock();
		} else {
			/* we have no control input (no FMU, no RC) */

			// XXX builtin failsafe would activate here
			control_count = 0;
		}
		//lib_lowprintf("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);

	/* this is for multicopters, etc. where manual override does not make sense */
	} else {
		/* if the fmu is available whe are good */
		if (system_state.mixer_fmu_available) {
			control_count = PX4IO_CONTROL_CHANNELS;
			control_values = &system_state.fmu_channel_data[0];
		/* we better shut everything off */
		} else {
			control_count = 0;
		}
	}

	/*
	 * Run the mixers if we have any control data at all.
	 */
	if (control_count > 0) {
		float	outputs[IO_SERVO_COUNT];
		unsigned mixed;

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

		/* scale to PWM and update the servo outputs as required */
		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
			if (i < mixed) {
				/* scale to servo output */
				system_state.servos[i] = (outputs[i] * 500.0f) + 1500;

			} else {
				/* set to zero to inhibit PWM pulse output */
				system_state.servos[i] = 0;
			}

			/*
			 * If we are armed, update the servo output.
			 */
			if (system_state.armed && system_state.arm_ok) {
				up_pwm_servo_set(i, system_state.servos[i]);
			}
		}
	}

	/*
	 * Decide whether the servos should be armed right now.
	 * A sufficient reason is armed state and either FMU or RC control inputs
	 */

	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);

	if (should_arm && !mixer_servos_armed) {
		/* need to arm, but not armed */
		up_pwm_servo_arm(true);
		mixer_servos_armed = true;

	} else if (!should_arm && mixer_servos_armed) {
		/* armed but need to disarm */
		up_pwm_servo_arm(false);
		mixer_servos_armed = false;
	}
}