Exemplo n.º 1
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;

}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &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;
	}

}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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]);
	}
}
Exemplo n.º 6
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);
		// if (updated) {
		// 	size_t num_outputs = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
		//
		// 	// Get updated actuator
		// 	// Only update actuator 1 for now
		// 	orb_copy(ORB_ID(actuator_controls_1), _actuator_controls_sub, &_controls[1]);
		//
		//
		// }

		_should_run = true;
	}

	// check if any activity remains, else stop
	if (!_should_run) {
		_running = false;
		return;
	}

	// re-queue ourselves to run again later
	_running = true;
	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
Exemplo n.º 7
0
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);
    }
}
Exemplo n.º 8
0
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]);
	}
}
Exemplo n.º 9
0
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;
	}
}
Exemplo n.º 10
0
bool MixerTest::mixerTest()
{
	/*
	 * PWM limit structure
	 */
	pwm_limit_t pwm_limit;
	bool should_arm = false;
	uint16_t r_page_servo_disarmed[output_max];
	uint16_t r_page_servo_control_min[output_max];
	uint16_t r_page_servo_control_max[output_max];
	uint16_t r_page_servos[output_max];
	uint16_t servo_predicted[output_max];
	int16_t reverse_pwm_mask = 0;

	bool load_ok = load_mixer(MIXER_PATH(IO_pass.mix), 8);

	if (!load_ok) {
		return load_ok;
	}

	/* execute the mixer */

	float	outputs[output_max];
	unsigned mixed;
	const int jmax = 5;

	pwm_limit_init(&pwm_limit);

	/* run through arming phase */
	for (unsigned i = 0; i < output_max; i++) {
		actuator_controls[i] = 0.1f;
		r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
		r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
	}

	//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");

	/* mix */
	should_prearm = true;
	mixed = mixer_group.mix(&outputs[0], output_max, nullptr);

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

	//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
	for (unsigned i = 0; i < mixed; i++) {

		//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

		if (i != actuator_controls_s::INDEX_THROTTLE) {
			if (r_page_servos[i] < r_page_servo_control_min[i]) {
				warnx("active servo < min");
				return false;
			}

		} else {
			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
				warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
				return false;
			}
		}
	}

	should_arm = true;
	should_prearm = false;

	/* simulate another orb_copy() from actuator controls */
	for (unsigned i = 0; i < output_max; i++) {
		actuator_controls[i] = 0.1f;
	}

	//PX4_INFO("ARMING TEST: STARTING RAMP");
	unsigned sleep_quantum_us = 10000;

	hrt_abstime starttime = hrt_absolute_time();
	unsigned sleepcount = 0;

	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {

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

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

		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
		for (unsigned i = 0; i < mixed; i++) {

			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

			/* check mixed outputs to be zero during init phase */
			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
			    r_page_servos[i] != r_page_servo_disarmed[i]) {
				PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
				return false;
			}

			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
			    r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
				PX4_ERR("ramp servo value mismatch");
				return false;
			}
		}

		usleep(sleep_quantum_us);
		sleepcount++;

		if (sleepcount % 10 == 0) {
			fflush(stdout);
		}
	}

	//PX4_INFO("ARMING TEST: NORMAL OPERATION");

	for (int j = -jmax; j <= jmax; j++) {

		for (unsigned i = 0; i < output_max; i++) {
			actuator_controls[i] = j / 10.0f + 0.1f * i;
			r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
			r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
			r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
		}

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

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

		//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);

		for (unsigned i = 0; i < mixed; i++) {
			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;

			if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
				fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
					(int)r_page_servos[i]);
				PX4_ERR("mixer violated predicted value");
				return false;
			}
		}
	}

	//PX4_INFO("ARMING TEST: DISARMING");

	starttime = hrt_absolute_time();
	sleepcount = 0;
	should_arm = false;

	while (hrt_elapsed_time(&starttime) < 600000) {

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

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

		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
		for (unsigned i = 0; i < mixed; i++) {

			//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

			/* check mixed outputs to be zero during init phase */
			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
				PX4_ERR("disarmed servo value mismatch");
				return false;
			}
		}

		usleep(sleep_quantum_us);
		sleepcount++;

		if (sleepcount % 10 == 0) {
			//printf(".");
			//fflush(stdout);
		}
	}

	//printf("\n");

	//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");

	starttime = hrt_absolute_time();
	sleepcount = 0;
	should_arm = true;

	while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {

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

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

		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
		for (unsigned i = 0; i < mixed; i++) {
			/* predict value */
			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;

			/* check ramp */

			//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);

			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
			    (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
			     r_page_servos[i] > servo_predicted[i])) {
				PX4_ERR("ramp servo value mismatch");
				return false;
			}

			/* check post ramp phase */
			if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
			    abs(servo_predicted[i] - r_page_servos[i]) > 2) {
				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
				PX4_ERR("mixer violated predicted value");
				return false;
			}
		}

		usleep(sleep_quantum_us);
		sleepcount++;

		if (sleepcount % 10 == 0) {
			//	printf(".");
			//	fflush(stdout);
		}
	}

	return true;
}
Exemplo n.º 11
0
void
TAP_ESC::cycle()
{

	if (!_initialized) {
		_current_update_rate = 0;
		/* advertise the mixed control outputs, insist on the first group output */
		_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
		_esc_feedback_pub = orb_advertise(ORB_ID(esc_report), &_esc_feedback);
		_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
		_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
		_initialized = true;
	}

	if (_groups_subscribed != _groups_required) {
		subscribe();
		_groups_subscribed = _groups_required;
		_current_update_rate = 0;
	}

	unsigned max_rate = _pwm_default_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", 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 */
	int ret = ::poll(_poll_fds, _poll_fds_num, 5);


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

	} else { /* update even in the case of a timeout, to check for test_motor commands */

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

		size_t num_outputs = _channels_count;

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


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

			/* disable unused ports by setting their output to NaN */
			for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}

			/* iterate actuators */
			for (unsigned i = 0; i < num_outputs; i++) {
				/* last resort: catch NaN, INF and out-of-band errors */
				if (i < _outputs.noutputs &&
				    PX4_ISFINITE(_outputs.output[i]) &&
				    _outputs.output[i] >= -1.0f &&
				    _outputs.output[i] <= 1.0f) {
					/* scale for PWM output 1000 - 2000us */
					_outputs.output[i] = 1600 + (350 * _outputs.output[i]);

				} else {
					/*
					 * 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] = RPMSTOPPED;
				}
			}

		} else {

			_outputs.noutputs = num_outputs;
			_outputs.timestamp = hrt_absolute_time();

			/* check for motor test commands */
			bool test_motor_updated;
			orb_check(_test_motor_sub, &test_motor_updated);

			if (test_motor_updated) {
				struct test_motor_s test_motor;
				orb_copy(ORB_ID(test_motor), _test_motor_sub, &test_motor);
				_outputs.output[test_motor.motor_number] = RPMSTOPPED + ((RPMMAX - RPMSTOPPED) * test_motor.value);
				PX4_INFO("setting motor %i to %.1lf", test_motor.motor_number,
					 (double)_outputs.output[test_motor.motor_number]);
			}

			/* set the invalid values to the minimum */
			for (unsigned i = 0; i < num_outputs; i++) {
				if (!PX4_ISFINITE(_outputs.output[i])) {
					_outputs.output[i] = RPMSTOPPED;
				}
			}

			/* disable unused ports by setting their output to NaN */
			for (size_t i = num_outputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}

		}

		const unsigned esc_count = num_outputs;
		float motor_out[TAP_ESC_MAX_MOTOR_NUM];

		// We need to remap from the system default to what PX4's normal
		// scheme is
		if (num_outputs == 6) {
			motor_out[0] = _outputs.output[3];
			motor_out[1] = _outputs.output[0];
			motor_out[2] = _outputs.output[4];
			motor_out[3] = _outputs.output[2];
			motor_out[4] = _outputs.output[1];
			motor_out[5] = _outputs.output[5];
			motor_out[6] = RPMSTOPPED;
			motor_out[7] = RPMSTOPPED;

		} else {

			// Use the system defaults
			for (int i = 0; i < esc_count; ++i) {
				motor_out[i] = _outputs.output[i];
			}
		}

		send_esc_outputs(motor_out, esc_count);
		read_data_from_uart();

		if (parse_tap_esc_feedback(&uartbuf, &_packet) == true) {
			if (_packet.msg_id == ESCBUS_MSG_ID_RUN_INFO) {
				RunInfoRepsonse &feed_back_data = _packet.d.rspRunInfo;

				if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
					_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
//					_esc_feedback.esc[feed_back_data.channelID].esc_voltage = feed_back_data.voltage;
					_esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus;
					_esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP;
					// printf("vol is %d\n",feed_back_data.voltage );
					// printf("speed is %d\n",feed_back_data.speed );

					_esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL;
					_esc_feedback.counter++;
					_esc_feedback.esc_count = esc_count;

					_esc_feedback.timestamp = hrt_absolute_time();

					orb_publish(ORB_ID(esc_status), _esc_feedback_pub, &_esc_feedback);
				}
			}
		}

		/* and publish for anyone that cares to see */
		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);

	}

	bool updated;

	orb_check(_armed_sub, &updated);

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

		if (_is_armed != _armed.armed) {
			/* reset all outputs */
			for (size_t i = 0; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
				_outputs.output[i] = NAN;
			}
		}

		/* do not obey the lockdown value, as lockdown is for PWMSim */
		_is_armed = _armed.armed;

	}


}
Exemplo n.º 12
0
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);
}
Exemplo n.º 13
0
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)) {

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

		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 */
		/* 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, 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);
		}
	}
}
Exemplo n.º 14
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));
}
Exemplo n.º 15
0
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]);
	}
}
Exemplo n.º 16
0
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;
	}
}