y2kare
April 24, 2017, 11:10pm
1
for my VTOL quad tailsitter I am using the elevons for yaw control in MC mode and the for YPR in FW mode so i was wondering will this be a valid mixer file if i combine groups 0 and 1
R: 4+ 10000 10000 0 0
mixer for the elevons
M: 4
O: 7500 7500 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
S: 1 0 -10000 -10000 0 -10000 10000
S: 1 1 10000 10000 0 -10000 10000
S: 1 2 -10000 -10000 0 -10000 10000
M: 4
O: 7500 7500 0 -10000 10000
S: 0 2 -10000 -10000 0 -10000 10000
S: 1 0 -10000 -10000 0 -10000 10000
S: 1 1 10000 10000 0 -10000 10000
S: 1 2 -10000 -10000 0 -10000 10000
dagar
May 2, 2017, 3:37pm
2
Just curious if you tried this? I wouldn’t expect it to work, but I’ve never tried. PX4 handles this kind of thing in the VTOL controller.
/****************************************************************************
*
* 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 tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
* @author David Vorsin <davidvorsin@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#define ARSP_YAW_CTRL_DISABLE 4.0f // airspeed at which we stop controlling yaw during a front transition
#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
using namespace matrix;
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_flag_was_in_trans_mode = false;
_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
_params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
}
void
Tailsitter::parameters_update()
{
float v;
/* vtol front transition phase 2 duration */
param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
_params_tailsitter.front_trans_dur_p2 = v;
param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
_params_tailsitter.fw_pitch_sp_offset = math::radians(v);
}
void Tailsitter::update_vtol_state()
{
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting in MC control mode, picking up
* forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
Eulerf euler = Quatf(_v_att->q);
float pitch = euler.theta();
if (!_attc->is_fixed_wing_requested()) {
switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
case MC_MODE:
break;
case FW_MODE:
_vtol_schedule.flight_mode = TRANSITION_BACK;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case TRANSITION_FRONT_P1:
// failsafe into multicopter mode
_vtol_schedule.flight_mode = MC_MODE;
break;
case TRANSITION_BACK:
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached pitch angle to switch to MC mode
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
_vtol_schedule.flight_mode = MC_MODE;
}
break;
}
} else { // user switchig to FW mode
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
// initialise a front transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
break;
case FW_MODE:
break;
case TRANSITION_FRONT_P1: {
bool airspeed_condition_satisfied = _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed;
airspeed_condition_satisfied |= _params->airspeed_disabled;
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if ((airspeed_condition_satisfied && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
}
break;
}
case TRANSITION_BACK:
// failsafe into fixed wing mode
_vtol_schedule.flight_mode = FW_MODE;
break;
}
}
// map tailsitter specific control phases to simple control modes
switch (_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
_vtol_vehicle_status->vtol_in_trans_mode = false;
_flag_was_in_trans_mode = false;
break;
case TRANSITION_FRONT_P1:
_vtol_mode = TRANSITION_TO_FW;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
case TRANSITION_BACK:
_vtol_mode = TRANSITION_TO_MC;
_vtol_vehicle_status->vtol_in_trans_mode = true;
break;
}
}
void Tailsitter::update_transition_state()
{
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_flag_was_in_trans_mode) {
_flag_was_in_trans_mode = true;
if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
// calculate rotation axis for transition.
_q_trans_start = Quatf(_v_att->q);
Vector3f z = -_q_trans_start.dcm_z();
_trans_rot_axis = z.cross(Vector3f(0, 0, -1));
// as heading setpoint we choose the heading given by the direction the vehicle points
float yaw_sp = atan2f(z(1), z(0));
// the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
// the yaw setpoint and zero roll since we want wings level transition
_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
// multirotor frame
_q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// initial attitude setpoint for the transition should be with wings level
_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
_trans_rot_axis = -x.cross(Vector3f(0, 0, -1));
}
_q_trans_sp = _q_trans_start;
}
// tilt angle (zero if vehicle nose points up (hover))
float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * _q_trans_sp(
2) + _q_trans_sp(3) * _q_trans_sp(3));
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
if (!flag_idle_mc) {
flag_idle_mc = set_idle_mc();
}
if (tilt > 0.01f) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
}
_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_v_att_sp->timestamp = hrt_absolute_time();
const Eulerf euler_sp(_q_trans_sp);
_v_att_sp->roll_body = euler_sp.phi();
_v_att_sp->pitch_body = euler_sp.theta();
_v_att_sp->yaw_body = euler_sp.psi();
_q_trans_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
}
void Tailsitter::waiting_on_tecs()
{
// copy the last trust value from the front transition
_v_att_sp->thrust_body[0] = _thrust_transition;
}
void Tailsitter::update_fw_state()
{
VtolType::update_fw_state();
// allow fw yawrate control via multirotor roll actuation. this is useful for vehicles
// which don't have a rudder to coordinate turns
if (_params->diff_thrust == 1) {
_mc_roll_weight = 1.0f;
}
}
/**
* Write data to actuator output topic.
*/
void Tailsitter::fill_actuator_outputs()
{
_actuators_out_0->timestamp = hrt_absolute_time();
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp = hrt_absolute_time();
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
* _mc_roll_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
_mc_yaw_weight;
if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
}
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) {
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0;
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0;
} else {
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
}
}
This file has been truncated. show original
The mixer file worked just fine. I had to simulate airspeed using a high-speed fan for fixed wing mode, that’s when the elevons started working.
Although I am working to enable differential rpm now for YAW in FW mode. I tried your suggestion to set _params_tiltrotor.diff_thrust to 1 but i didn’t notice any change in rpm ( i changed the diff_thrust_scale to 1 as well)