示例#1
0
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
	// send one last publication when destroyed, setting
	// all output to zero
	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	updatePublications();
}
void BlockSegwayController::update() {
	// wait for a sensor update, check for exit condition every 100 ms
	if (poll(&_attPoll, 1, 100) < 0) return; // poll error

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// check for sane values of dt
	// to prevent large control responses
	if (dt > 1.0f || dt < 0) return;

	// set dt for all child blocks
	setDt(dt);

	// check for new updates
	if (_param_update.updated()) updateParams();

	// get new information from subscriptions
	updateSubscriptions();

	// default all output to zero unless handled by mode
	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	// only update guidance in auto mode
	if (_status.main_state == MAIN_STATE_AUTO) {
		// update guidance
	}

	// compute speed command
	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);

	// handle autopilot modes
	if (_status.main_state == MAIN_STATE_AUTO ||
	    _status.main_state == MAIN_STATE_ALTCTL ||
	    _status.main_state == MAIN_STATE_POSCTL) {
		_actuators.control[0] = spdCmd;
		_actuators.control[1] = spdCmd;

	} else if (_status.main_state == MAIN_STATE_MANUAL) {
		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
			_actuators.control[CH_LEFT] = _manual.throttle;
			_actuators.control[CH_RIGHT] = _manual.pitch;

		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
			_actuators.control[0] = spdCmd;
			_actuators.control[1] = spdCmd;
		}
	}

	// update all publications
	updatePublications();

}
示例#3
0
void BlockMultiModeBacksideAutopilot::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	if (poll(&_attPoll, 1, 100) < 0) return; // poll error

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// check for sane values of dt
	// to prevent large control responses
	if (dt > 1.0f || dt < 0) return;

	// set dt for all child blocks
	setDt(dt);

	// store old position command before update if new command sent
	if (_posCmd.updated()) {
		_lastPosCmd = _posCmd.getData();
	}

	// check for new updates
	if (_param_update.updated()) updateParams();

	// get new information from subscriptions
	updateSubscriptions();

	// default all output to zero unless handled by mode
	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	// only update guidance in auto mode
	if (_status.state_machine == SYSTEM_STATE_AUTO) {
		// update guidance
		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
	}

	// XXX handle STABILIZED (loiter on spot) as well
	// once the system switches from manual or auto to stabilized
	// the setpoint should update to loitering around this position

	// handle autopilot modes
	if (_status.state_machine == SYSTEM_STATE_AUTO ||
	    _status.state_machine == SYSTEM_STATE_STABILIZED) {

		// update guidance
		_guide.update(_pos, _att, _posCmd, _lastPosCmd);

		// calculate velocity, XXX should be airspeed, but using ground speed for now
		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);

		// altitude hold
		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);

		// heading hold
		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
		float pCmd = _phi2P.update(phiCmd - _att.roll);

		// velocity hold
		// negative sign because nose over to increase speed
		float thetaCmd = _theLimit.update(-_v2Theta.update(
				_vLimit.update(_vCmd.get()) - v));
		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

		// yaw rate cmd
		float rCmd = 0;

		// stabilization
		_stabilization.update(pCmd, qCmd, rCmd,
				      _att.rollspeed, _att.pitchspeed, _att.yawspeed);

		// output
		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
		_actuators.control[CH_THR] = dThrottle + _trimThr.get();

		// XXX limit throttle to manual setting (safety) for now.
		// If it turns out to be confusing, it can be removed later once
		// a first binary release can be targeted.
		// This is not a hack, but a design choice.

		/* do not limit in HIL */
		if (!_status.flag_hil_enabled) {
			/* limit to value of manual throttle */
			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
						     _actuators.control[CH_THR] : _manual.throttle;
		}

	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {

		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
			_actuators.control[CH_AIL] = _manual.roll;
			_actuators.control[CH_ELV] = _manual.pitch;
			_actuators.control[CH_RDR] = _manual.yaw;
			_actuators.control[CH_THR] = _manual.throttle;

		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {

			// calculate velocity, XXX should be airspeed, but using ground speed for now
			float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);

			// pitch channel -> rate of climb
			// TODO, might want to put a gain on this, otherwise commanding
			// from +1 -> -1 m/s for rate of climb
			//float dThrottle = _roc2Thr.update(
			//_rocMax.get()*_manual.pitch - _pos.vz);

			// roll channel -> bank angle
			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
			float pCmd = _phi2P.update(phiCmd - _att.roll);

			// throttle channel -> velocity
			// negative sign because nose over to increase speed
			float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
			float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

			// yaw rate cmd
			float rCmd = 0;

			// stabilization
			_stabilization.update(pCmd, qCmd, rCmd,
					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);

			// output
			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();

			// currenlty using manual throttle
			// XXX if you enable this watch out, vz might be very noisy
			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
			_actuators.control[CH_THR] = _manual.throttle;

			// XXX limit throttle to manual setting (safety) for now.
			// If it turns out to be confusing, it can be removed later once
			// a first binary release can be targeted.
			// This is not a hack, but a design choice.

			/* do not limit in HIL */
			if (!_status.flag_hil_enabled) {
				/* limit to value of manual throttle */
				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
							     _actuators.control[CH_THR] : _manual.throttle;
			}
		}

		// body rates controller, disabled for now
		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {

			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);

			_actuators.control[CH_AIL] = _stabilization.getAileron();
			_actuators.control[CH_ELV] = _stabilization.getElevator();
			_actuators.control[CH_RDR] = _stabilization.getRudder();
			_actuators.control[CH_THR] = _manual.throttle;
		}
	}

	// update all publications
	updatePublications();
}
示例#4
0
void BlockMultiModeBacksideAutopilot::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	if (poll(&_attPoll, 1, 100) < 0) return; // poll error

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// check for sane values of dt
	// to prevent large control responses
	if (dt > 1.0f || dt < 0) return;

	// set dt for all child blocks
	setDt(dt);

	// store old position command before update if new command sent
	if (_posCmd.updated()) {
		_lastPosCmd = _posCmd.getData();
	}

	// check for new updates
	if (_param_update.updated()) updateParams();

	// get new information from subscriptions
	updateSubscriptions();

	// default all output to zero unless handled by mode
	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	// handle autopilot modes
	if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
		_stabilization.update(
			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
		_actuators.control[CH_AIL] = _stabilization.getAileron();
		_actuators.control[CH_ELV] = _stabilization.getElevator();
		_actuators.control[CH_RDR] = _stabilization.getRudder();
		_actuators.control[CH_THR] = _manual.throttle;

	} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
		// update guidance
		_guide.update(_pos, _att, _posCmd, _lastPosCmd);

		// calculate velocity, XXX should be airspeed, but using ground speed for now
		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);

		// commands
		float rCmd = 0;

		_backsideAutopilot.update(
			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
			_pos.alt, v,
			_att.roll, _att.pitch, _att.yaw,
			_att.rollspeed, _att.pitchspeed, _att.yawspeed
		);
		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();

	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
		_actuators.control[CH_AIL] = _manual.roll;
		_actuators.control[CH_ELV] = _manual.pitch;
		_actuators.control[CH_RDR] = _manual.yaw;
		_actuators.control[CH_THR] = _manual.throttle;
	}

	// update all publications
	updatePublications();
}