text: /****************************************************************************
   *
   *   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 vtol_type.cpp
  *
  * @author Roman Bapst 		<bapstroman@gmail.com>
  * @author Andreas Antener	<andreas@uaventure.com>
  *
  */
  
  #include "vtol_type.h"
  #include "vtol_att_control_main.h"
  
  #include <float.h>
  #include <px4_platform_common/defines.h>
  #include <matrix/math.hpp>
  
  using namespace matrix;
  
  
  VtolType::VtolType(VtolAttitudeControl *att_controller) :
  	_attc(att_controller),
  	_vtol_mode(mode::ROTARY_WING)
  {
  	_v_att = _attc->get_att();
  	_v_att_sp = _attc->get_att_sp();
  	_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
  	_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
  	_v_control_mode = _attc->get_control_mode();
  	_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
  	_actuators_out_0 = _attc->get_actuators_out0();
  	_actuators_out_1 = _attc->get_actuators_out1();
  	_actuators_mc_in = _attc->get_actuators_mc_in();
  	_actuators_fw_in = _attc->get_actuators_fw_in();
  	_local_pos = _attc->get_local_pos();
  	_local_pos_sp = _attc->get_local_pos_sp();
  	_airspeed_validated = _attc->get_airspeed();
  	_tecs_status = _attc->get_tecs_status();
  	_land_detected = _attc->get_land_detected();
  	_params = _attc->get_params();
  
  	for (auto &pwm_max : _max_mc_pwm_values.values) {
  		pwm_max = PWM_DEFAULT_MAX;
  	}
  
  	for (auto &pwm_disarmed : _disarmed_pwm_values.values) {
  		pwm_disarmed = PWM_MOTOR_OFF;
  	}
  }
  
  bool VtolType::init()
  {
  	const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
  
  	int fd = px4_open(dev, 0);
  
  	if (fd < 0) {
  		PX4_ERR("can't open %s", dev);
  		return false;
  	}
  
  	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values);
  	_current_max_pwm_values = _max_mc_pwm_values;
  
  	if (ret != PX4_OK) {
  		PX4_ERR("failed getting max values");
  		px4_close(fd);
  		return false;
  	}
  
  	ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values);
  
  	if (ret != PX4_OK) {
  		PX4_ERR("failed getting min values");
  		px4_close(fd);
  		return false;
  	}
  
  	ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values);
  
  	if (ret != PX4_OK) {
  		PX4_ERR("failed getting disarmed values");
  		px4_close(fd);
  		return false;
  	}
  
  	px4_close(fd);
  
  	_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id);
  	_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off);
  
  
  	// in order to get the main motors we take all motors and clear the alternate motor bits
  	for (int i = 0; i < 8; i++) {
  		if (_alternate_motor_channel_bitmap & (1 << i)) {
  			_main_motor_channel_bitmap &= ~(1 << i);
  		}
  	}
  
  	return true;
  
  }
  
  void VtolType::update_mc_state()
  {
  	if (!_flag_idle_mc) {
  		_flag_idle_mc = set_idle_mc();
  	}
  
  	resetAccelToPitchPitchIntegrator();
  
  	VtolType::set_all_motor_state(motor_state::ENABLED);
  
  	// copy virtual attitude setpoint to real attitude setpoint
  	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
  
  	_mc_roll_weight = 1.0f;
  	_mc_pitch_weight = 1.0f;
  	_mc_yaw_weight = 1.0f;
  	_mc_throttle_weight = 1.0f;
  }
  
  void VtolType::update_fw_state()
  {
  	if (_flag_idle_mc) {
  		_flag_idle_mc = !set_idle_fw();
  	}
  
  	resetAccelToPitchPitchIntegrator();
  
  	VtolType::set_alternate_motor_state(motor_state::DISABLED);
  
  	// copy virtual attitude setpoint to real attitude setpoint
  	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
  	_mc_roll_weight = 0.0f;
  	_mc_pitch_weight = 0.0f;
  	_mc_yaw_weight = 0.0f;
  
  	// tecs didn't publish an update yet after the transition
  	if (_tecs_status->timestamp < _trans_finished_ts) {
  		_tecs_running = false;
  
  	} else if (!_tecs_running) {
  		_tecs_running = true;
  		_tecs_running_ts = hrt_absolute_time();
  	}
  
  	// TECS didn't publish yet or the position controller didn't publish yet AFTER tecs
  	// only wait on TECS we're in a mode where it is actually running
  	if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts))
  	    && _v_control_mode->flag_control_altitude_enabled) {
  
  		waiting_on_tecs();
  	}
  
  	check_quadchute_condition();
  }
  
  void VtolType::update_transition_state()
  {
  	hrt_abstime t_now = hrt_absolute_time();
  	_transition_dt = (float)(t_now - _last_loop_ts) / 1e6f;
  	_transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f);
  	_last_loop_ts = t_now;
  
  
  
  	check_quadchute_condition();
  }
  
  float VtolType::update_and_get_backtransition_pitch_sp()
  {
  	// maximum up or down pitch the controller is allowed to demand
  	const float pitch_lim = 0.3f;
  	const Eulerf euler(Quatf(_v_att->q));
  
  	const float track = atan2f(_local_pos->vy, _local_pos->vx);
  	const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
  
  	// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
  	const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
  
  	const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
  
  	float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
  
  	if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
  	    (pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f)) {
  		integrator_input = 0.0f;
  	}
  
  	_accel_to_pitch_integ += integrator_input * _transition_dt;
  
  
  	return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim);
  }
  
  bool VtolType::can_transition_on_ground()
  {
  	return !_v_control_mode->flag_armed || _land_detected->landed;
  }
  
  void VtolType::check_quadchute_condition()
  {
  	if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
  	    && !_quadchute_command_treated) {
  		_attc->quadchute("External command");
  		_quadchute_command_treated = true;
  		_attc->reset_immediate_transition();
  
  	} else {
  		_quadchute_command_treated = false;
  	}
  
  	if (!_tecs_running) {
  		// reset the filtered height rate and heigh rate setpoint if TECS is not running
  		_ra_hrate = 0.0f;
  		_ra_hrate_sp = 0.0f;
  	}
  
  	if (_v_control_mode->flag_armed && !_land_detected->landed) {
  		Eulerf euler = Quatf(_v_att->q);
  
  		// fixed-wing minimum altitude
  		if (_params->fw_min_alt > FLT_EPSILON) {
  
  			if (-(_local_pos->z) < _params->fw_min_alt) {
  				_attc->quadchute("Minimum altitude breached");
  			}
  		}
  
  		// adaptive quadchute
  		if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
  
  			// We use tecs for tracking in FW and local_pos_sp during transitions
  			if (_tecs_running) {
  				// 1 second rolling average
  				_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
  				_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
  
  				// are we dropping while requesting significant ascend?
  				if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) &&
  				    (_ra_hrate < -1.0f) &&
  				    (_ra_hrate_sp > 1.0f)) {
  
  					_attc->quadchute("Loss of altitude");
  				}
  
  			} else {
  				const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err);
  				const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
  
  				if (height_error && height_rate_error) {
  					_attc->quadchute("Large altitude error");
  				}
  			}
  		}
  
  		// fixed-wing maximum pitch angle
  		if (_params->fw_qc_max_pitch > 0) {
  
  			if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
  				_attc->quadchute("Maximum pitch angle exceeded");
  			}
  		}
  
  		// fixed-wing maximum roll angle
  		if (_params->fw_qc_max_roll > 0) {
  
  			if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
  				_attc->quadchute("Maximum roll angle exceeded");
  			}
  		}
  	}
  }
  
  bool VtolType::set_idle_mc()
  {
  	unsigned pwm_value = _params->idle_pwm_mc;
  	struct pwm_output_values pwm_values {};
  
  	for (int i = 0; i < num_outputs_max; i++) {
  		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
  			pwm_values.values[i] = pwm_value;
  
  		} else {
  			pwm_values.values[i] = _min_mc_pwm_values.values[i];
  		}
  
  		pwm_values.channel_count++;
  	}
  
  	return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM);
  }
  
  bool VtolType::set_idle_fw()
  {
  	struct pwm_output_values pwm_values {};
  
  	for (int i = 0; i < num_outputs_max; i++) {
  		if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
  			pwm_values.values[i] = PWM_DEFAULT_MIN;
  
  		} else {
  			pwm_values.values[i] = _min_mc_pwm_values.values[i];
  		}
  
  		pwm_values.channel_count++;
  	}
  
  	return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM);
  }
  
  bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
  {
  	const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
  
  	int fd = px4_open(dev, 0);
  
  	if (fd < 0) {
  		PX4_WARN("can't open %s", dev);
  		return false;
  	}
  
  	int ret;
  
  	if (type == pwm_limit_type::TYPE_MINIMUM) {
  		ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
  
  	} else {
  		ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
  	}
  
  	px4_close(fd);
  
  
  	if (ret != OK) {
  		PX4_DEBUG("failed setting max values");
  		return false;
  	}
  
  	return true;
  }
  
  void VtolType::set_all_motor_state(const motor_state target_state, const int value)
  {
  	set_main_motor_state(target_state, value);
  	set_alternate_motor_state(target_state, value);
  }
  
  void VtolType::set_main_motor_state(const motor_state target_state, const int value)
  {
  	if (_main_motor_state != target_state) {
  
  		if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) {
  			_main_motor_state = target_state;
  		}
  	}
  }
  
  void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
  {
  	if (_alternate_motor_state != target_state) {
  
  		if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) {
  			_alternate_motor_state = target_state;
  		}
  	}
  }
  
  bool VtolType::set_motor_state(const motor_state target_state, const int32_t channel_bitmap,  const int value)
  {
  	switch (target_state) {
  	case motor_state::ENABLED:
  		for (int i = 0; i < num_outputs_max; i++) {
  			if (is_channel_set(i, channel_bitmap)) {
  				_current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i];
  			}
  		}
  
  		break;
  
  	case motor_state::DISABLED:
  		for (int i = 0; i < num_outputs_max; i++) {
  			if (is_channel_set(i, channel_bitmap)) {
  				_current_max_pwm_values.values[i] = _disarmed_pwm_values.values[i];
  			}
  		}
  
  		break;
  
  	case motor_state::IDLE:
  
  		for (int i = 0; i < num_outputs_max; i++) {
  			if (is_channel_set(i, channel_bitmap)) {
  				_current_max_pwm_values.values[i] = _params->idle_pwm_mc;
  			}
  		}
  
  		break;
  
  	case motor_state::VALUE:
  		for (int i = 0; i < num_outputs_max; i++) {
  			if (is_channel_set(i, channel_bitmap)) {
  				_current_max_pwm_values.values[i] = value;
  			}
  		}
  
  		break;
  	}
  
  	_current_max_pwm_values.channel_count = num_outputs_max;
  
  	return apply_pwm_limits(_current_max_pwm_values, pwm_limit_type::TYPE_MAXIMUM);
  }
  
  int VtolType::generate_bitmap_from_channel_numbers(const int channels)
  {
  	int channel_bitmap = 0;
  	int channel_numbers = channels;
  
  	int tmp;
  
  	for (int i = 0; i < num_outputs_max; ++i) {
  		tmp = channel_numbers % 10;
  
  		if (tmp == 0) {
  			break;
  		}
  
  		channel_bitmap |= 1 << (tmp - 1);
  		channel_numbers = channel_numbers / 10;
  	}
  
  	return channel_bitmap;
  }
  
  bool VtolType::is_channel_set(const int channel, const int bitmap)
  {
  	return bitmap & (1 << channel);
  }
  
  
  float VtolType::pusher_assist()
  {
  	// Altitude above ground is distance sensor altitude if available, otherwise local z-position
  	float dist_to_ground = -_local_pos->z;
  
  	if (_local_pos->dist_bottom_valid) {
  		dist_to_ground = _local_pos->dist_bottom;
  	}
  
  	// disable pusher assist depending on setting of forward_thrust_enable_mode:
  	switch (_params->vt_forward_thrust_enable_mode) {
  	case DISABLE: // disable in all modes
  		return 0.0f;
  		break;
  
  	case ENABLE_WITHOUT_LAND: // disable in land mode
  		if (_attc->get_pos_sp_triplet()->current.valid
  		    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
  		    && _v_control_mode->flag_control_auto_enabled) {
  			return 0.0f;
  		}
  
  		break;
  
  	case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
  		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) {
  			return 0.0f;
  		}
  
  		break;
  
  	case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
  		if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) {
  			return 0.0f;
  		}
  
  		break;
  
  	case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land mode
  		if ((_attc->get_pos_sp_triplet()->current.valid
  		     && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
  		     && _v_control_mode->flag_control_auto_enabled) ||
  		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) {
  			return 0.0f;
  		}
  
  		break;
  
  	case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or in land mode
  		if ((_attc->get_pos_sp_triplet()->current.valid
  		     && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
  		     && _v_control_mode->flag_control_auto_enabled) ||
  		    (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) {
  			return 0.0f;
  		}
  
  		break;
  	}
  
  	// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
  	// then the pusher-for-pitch strategy is disabled and we can return
  	if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
  			|| _v_control_mode->flag_control_altitude_enabled)) {
  		return 0.0f;
  	}
  
  	// Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive)
  	if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
  		return 0.0f;
  	}
  
  	const Dcmf R(Quatf(_v_att->q));
  	const Dcmf R_sp(Quatf(_v_att_sp->q_d));
  	const Eulerf euler(R);
  	const Eulerf euler_sp(R_sp);
  
  	// direction of desired body z axis represented in earth frame
  	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.
  	Dcmf R_yaw = 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 down
  	float pitch_down = atan2f(body_z_sp(0), body_z_sp(2));
  
  	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
  	float forward_thrust = 0.0f;
  
  	// only allow pitching down up to threshold, the rest of the desired
  	// forward acceleration will be compensated by the pusher/tilt
  	if (pitch_down < -_params->down_pitch_max) {
  		// desired roll angle in heading frame stays the same
  		float roll_new = -asinf(body_z_sp(1));
  
  		forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale;
  		// limit forward actuation to [0, 0.9]
  		forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f);
  
  		// return the vehicle to level position
  		float pitch_new = 0.0f;
  
  		// create corrected desired body z axis in heading frame
  		const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
  		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.
  		const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
  		const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
  		tilt_new = R_yaw_correction * tilt_new;
  
  		// now extract roll and pitch setpoints
  		_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
  		_v_att_sp->roll_body = -asinf(tilt_new(1));
  
  		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
  		q_sp.copyTo(_v_att_sp->q_d);
  	}
  
  	return forward_thrust;
  
  }
  