PX4-Autopilot/src/modules/vtol_att_control/standard.cpp

/****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file standard.cpp
 *
 * @author Simon Wilks		<simon@uaventure.com>
 * @author Roman Bapst		<bapstroman@gmail.com>
 * @author Andreas Antener	<andreas@uaventure.com>
 * @author Sander Smeets	<sander@droneslab.com>
 *
*/

#include "standard.h"
#include "vtol_att_control_main.h"

Standard::Standard(VtolAttitudeControl *attc) :
	VtolType(attc),
	_flag_enable_mc_motors(true),
	_pusher_throttle(0.0f),
	_airspeed_trans_blend_margin(0.0f)
{
	_vtol_schedule.flight_mode = MC_MODE;
	_vtol_schedule.transition_start = 0;
	_pusher_active = false;

	_mc_roll_weight = 1.0f;
	_mc_pitch_weight = 1.0f;
	_mc_yaw_weight = 1.0f;
	_mc_throttle_weight = 1.0f;

	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
	_params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
	_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
	_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
	_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
	_params_handles_standard.airspeed_mode = param_find("FW_ARSP_MODE");
}

Standard::~Standard()
{
}

int
Standard::parameters_update()
{
	float v;
	int i;

	/* duration of a forwards transition to fw mode */
	param_get(_params_handles_standard.front_trans_dur, &v);
	_params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f);

	/* duration of a back transition to mc mode */
	param_get(_params_handles_standard.back_trans_dur, &v);
	_params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f);

	/* target throttle value for pusher motor during the transition to fw mode */
	param_get(_params_handles_standard.pusher_trans, &v);
	_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);

	/* airspeed at which we should switch to fw mode */
	param_get(_params_handles_standard.airspeed_trans, &v);
	_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);

	/* airspeed at which we start blending mc/fw controls */
	param_get(_params_handles_standard.airspeed_blend, &v);
	_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);

	_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;

	/* timeout for transition to fw mode */
	param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout);

	/* minimum time for transition to fw mode */
	param_get(_params_handles_standard.front_trans_time_min, &_params_standard.front_trans_time_min);

	/* maximum down pitch allowed */
	param_get(_params_handles_standard.down_pitch_max, &v);
	_params_standard.down_pitch_max = math::radians(v);

	/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
	param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);

	/* airspeed mode */
	param_get(_params_handles_standard.airspeed_mode, &i);
	_params_standard.airspeed_mode = math::constrain(i, 0, 2);



	return OK;
}

void Standard::update_vtol_state()
{
	parameters_update();

	/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
	 * forward speed. After the vehicle has picked up enough speed the rotors shutdown.
	 * For the back transition the pusher motor is immediately stopped and rotors reactivated.
	 */

	if (!_attc->is_fixed_wing_requested()) {
		// the transition to fw mode switch is off
		if (_vtol_schedule.flight_mode == MC_MODE) {
			// in mc mode
			_vtol_schedule.flight_mode = MC_MODE;
			_mc_roll_weight = 1.0f;
			_mc_pitch_weight = 1.0f;
			_mc_yaw_weight = 1.0f;
			_mc_throttle_weight = 1.0f;

		} else if (_vtol_schedule.flight_mode == FW_MODE) {
			// transition to mc mode
			if (_vtol_vehicle_status->vtol_transition_failsafe == true) {
				// Failsafe event, engage mc motors immediately
				_vtol_schedule.flight_mode = MC_MODE;
				_flag_enable_mc_motors = true;

			} else {
				// Regular backtransition
				_vtol_schedule.flight_mode = TRANSITION_TO_MC;
				_flag_enable_mc_motors = true;
				_vtol_schedule.transition_start = hrt_absolute_time();
			}

		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
			// failsafe back to mc mode
			_vtol_schedule.flight_mode = MC_MODE;
			_mc_roll_weight = 1.0f;
			_mc_pitch_weight = 1.0f;
			_mc_yaw_weight = 1.0f;
			_mc_throttle_weight = 1.0f;

		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
			// transition to MC mode if transition time has passed
			// XXX: base this on XY hold velocity of MC
			if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
			    (_params_standard.back_trans_dur * 1000000.0f)) {
				_vtol_schedule.flight_mode = MC_MODE;
			}
		}

		// the pusher motor should never be powered when in or transitioning to mc mode
		_pusher_throttle = 0.0f;

	} else {
		// the transition to fw mode switch is on
		if (_vtol_schedule.flight_mode == MC_MODE) {
			// start transition to fw mode
			_vtol_schedule.flight_mode = TRANSITION_TO_FW;
			_vtol_schedule.transition_start = hrt_absolute_time();

		} else if (_vtol_schedule.flight_mode == FW_MODE) {
			// in fw mode
			_vtol_schedule.flight_mode = FW_MODE;
			_mc_roll_weight = 0.0f;
			_mc_pitch_weight = 0.0f;
			_mc_yaw_weight = 0.0f;
			_mc_throttle_weight = 0.0f;

		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
			if (((_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED ||
			      _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
			    can_transition_on_ground()) {
				_vtol_schedule.flight_mode = FW_MODE;
				// we can turn off the multirotor motors now
				_flag_enable_mc_motors = false;
				// don't set pusher throttle here as it's being ramped up elsewhere
				_trans_finished_ts = hrt_absolute_time();
			}

		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
			// transitioning to mc mode & transition switch on - failsafe back into fw mode
			_vtol_schedule.flight_mode = FW_MODE;
		}
	}

	// map specific control phases to simple control modes
	switch (_vtol_schedule.flight_mode) {
	case MC_MODE:
		_vtol_mode = ROTARY_WING;
		break;

	case FW_MODE:
		_vtol_mode = FIXED_WING;
		break;

	case TRANSITION_TO_FW:
	case TRANSITION_TO_MC:
		_vtol_mode = TRANSITION;
		break;
	}
}

void Standard::update_transition_state()
{
	VtolType::update_transition_state();

	// copy virtual attitude setpoint to real attitude setpoint
	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));

	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
		if (_params_standard.front_trans_dur <= 0.0f) {
			// just set the final target throttle value
			_pusher_throttle = _params_standard.pusher_trans;

		} else if (_pusher_throttle <= _params_standard.pusher_trans) {
			// ramp up throttle to the target throttle value
			_pusher_throttle = _params_standard.pusher_trans *
					   (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
		}

		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
		if (_airspeed_trans_blend_margin > 0.0f &&
		    _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend &&
		    (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
		   ) {
			float weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
				       _airspeed_trans_blend_margin;
			_mc_roll_weight = weight;
			_mc_pitch_weight = weight;
			_mc_yaw_weight = weight;
			_mc_throttle_weight = weight;

			// time based blending when no airspeed sensor is set

		} else if (_params_standard.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED &&
			   (float)hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1000000.0f)
			  ) {
			float weight = 1.0f - (float)(hrt_elapsed_time(&_vtol_schedule.transition_start) /
						      (_params_standard.front_trans_time_min * 1000000.0f));
			_mc_roll_weight = weight;
			_mc_pitch_weight = weight;
			_mc_yaw_weight = weight;
			_mc_throttle_weight = weight;


		} else {
			// at low speeds give full weight to mc
			_mc_roll_weight = 1.0f;
			_mc_pitch_weight = 1.0f;
			_mc_yaw_weight = 1.0f;
			_mc_throttle_weight = 1.0f;
		}

		// check front transition timeout
		if (_params_standard.front_trans_timeout > FLT_EPSILON) {
			if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) {
				// transition timeout occured, abort transition
				_attc->abort_front_transition();
			}
		}

	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
		// continually increase mc attitude control as we transition back to mc mode
		if (_params_standard.back_trans_dur > 0.0f) {
			float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
					1000000.0f);
			_mc_roll_weight = weight;
			_mc_pitch_weight = weight;
			_mc_yaw_weight = weight;
			_mc_throttle_weight = weight;

		} else {
			_mc_roll_weight = 1.0f;
			_mc_pitch_weight = 1.0f;
			_mc_yaw_weight = 1.0f;
			_mc_throttle_weight = 1.0f;
		}

		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
		if (_flag_enable_mc_motors) {
			set_max_mc(2000);
			set_idle_mc();
			_flag_enable_mc_motors = false;
		}
	}

	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
	_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
	_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
}

void Standard::update_mc_state()
{
	VtolType::update_mc_state();

	// if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return
	if (_params_standard.forward_thrust_scale < FLT_EPSILON) {
		return;
	}

	matrix::Dcmf R(matrix::Quatf(_v_att->q));
	matrix::Dcmf R_sp(&_v_att_sp->R_body[0]);
	matrix::Eulerf euler(R);
	matrix::Eulerf euler_sp(R_sp);
	_pusher_throttle = 0.0f;

	// direction of desired body z axis represented in earth frame
	matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

	// rotate desired body z axis into new frame which is rotated in z by the current
	// heading of the vehicle. we refer to this as the heading frame.
	matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
	body_z_sp = R_yaw * body_z_sp;
	body_z_sp.normalize();

	// calculate the desired pitch seen in the heading frame
	// this value corresponds to the amount the vehicle would try to pitch forward
	float pitch_forward = asinf(body_z_sp(0));

	// only allow pitching forward up to threshold, the rest of the desired
	// forward acceleration will be compensated by the pusher
	if (pitch_forward < -_params_standard.down_pitch_max) {
		// desired roll angle in heading frame stays the same
		float roll_new = -atan2f(body_z_sp(1), body_z_sp(2));

		_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust;

		// limit desired pitch
		float pitch_new = -_params_standard.down_pitch_max;

		// create corrected desired body z axis in heading frame
		matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
		matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));

		// rotate the vector into a new frame which is rotated in z by the desired heading
		// with respect to the earh frame.
		float yaw_error = _wrap_pi(euler_sp(2) - euler(2));
		matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
		tilt_new = R_yaw_correction * tilt_new;

		// now extract roll and pitch setpoints
		float pitch = asinf(tilt_new(0));
		float roll = -atan2f(tilt_new(1), tilt_new(2));
		R_sp = matrix::Eulerf(roll, pitch, euler_sp(2));
		matrix::Quatf q_sp(R_sp);
		memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body));
		memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d));
	}

	_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;

}

void Standard::update_fw_state()
{
	VtolType::update_fw_state();

	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
	if (!_flag_enable_mc_motors) {
		set_max_mc(950);
		set_idle_fw();  // force them to stop, not just idle
		_flag_enable_mc_motors = true;
	}
}

/**
 * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
 * what proportion of control should be applied to each of the control groups (mc and fw).
 */
void Standard::fill_actuator_outputs()
{
	/* multirotor controls */
	_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
			* _mc_roll_weight;	// roll

	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;	// pitch


	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
			_mc_yaw_weight;	// yaw
	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;	// throttle

	/* fixed wing controls */
	_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
			* (1 - _mc_roll_weight);	//roll
	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
		(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);	//pitch
	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
			* (1 - _mc_yaw_weight);	// yaw

	_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;

	// set the fixed wing throttle control
	if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) {
		// take the throttle value commanded by the fw controller
		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];

	} else {
		// otherwise we may be ramping up the throttle during the transition to fw mode
		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
	}
}

void
Standard::waiting_on_tecs()
{
	// keep thrust from transition
	_v_att_sp->thrust = _pusher_throttle;
};

/**
* Disable all multirotor motors when in fw mode.
*/
void
Standard::set_max_mc(unsigned pwm_value)
{
	int ret;
	unsigned servo_count;
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count = _params->vtol_motor_count;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting max values");
	}

	px4_close(fd);
}