tailsitter.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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 = vtol_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.
    */

    float pitch = Eulerf(Quatf(_v_att->q)).theta();

    if (!_attc->is_fixed_wing_requested()) {

        switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
        case vtol_mode::MC_MODE:
            break;

        case vtol_mode::FW_MODE:
            _vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
            _vtol_schedule.transition_start = hrt_absolute_time();
            break;

        case vtol_mode::TRANSITION_FRONT_P1:
            // failsafe into multicopter mode
            _vtol_schedule.flight_mode = vtol_mode::MC_MODE;
            break;

        case vtol_mode::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 = vtol_mode::MC_MODE;
            }

            break;
        }

    } else {  // user switchig to FW mode

        switch (_vtol_schedule.flight_mode) {
        case vtol_mode::MC_MODE:
            // initialise a front transition
            _vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
            _vtol_schedule.transition_start = hrt_absolute_time();
            break;

        case vtol_mode::FW_MODE:
            break;

        case vtol_mode::TRANSITION_FRONT_P1: {


                const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->equivalent_airspeed_m_s)
                        && !_params->airspeed_disabled;

                bool transition_to_fw = false;

                if (pitch <= PITCH_TRANSITION_FRONT_P1) {
                    if (airspeed_triggers_transition) {
                        transition_to_fw = _airspeed_validated->equivalent_airspeed_m_s >= _params->transition_airspeed;

                    } else {
                        transition_to_fw = true;
                    }
                }

                transition_to_fw |= can_transition_on_ground();

                if (transition_to_fw) {
                    _vtol_schedule.flight_mode = vtol_mode::FW_MODE;
                }

                break;
            }

        case vtol_mode::TRANSITION_BACK:
            // failsafe into fixed wing mode
            _vtol_schedule.flight_mode = vtol_mode::FW_MODE;
            break;
        }
    }

    // map tailsitter specific control phases to simple control modes
    switch (_vtol_schedule.flight_mode) {
    case vtol_mode::MC_MODE:
        _vtol_mode = mode::ROTARY_WING;
        _vtol_vehicle_status->vtol_in_trans_mode = false;
        _flag_was_in_trans_mode = false;
        break;

    case vtol_mode::FW_MODE:
        _vtol_mode = mode::FIXED_WING;
        _vtol_vehicle_status->vtol_in_trans_mode = false;
        _flag_was_in_trans_mode = false;
        break;

    case vtol_mode::TRANSITION_FRONT_P1:
        _vtol_mode = mode::TRANSITION_TO_FW;
        _vtol_vehicle_status->vtol_in_trans_mode = true;
        break;

    case vtol_mode::TRANSITION_BACK:
        _vtol_mode = 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 == vtol_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 == vtol_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 == vtol_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 == vtol_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 == vtol_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 == vtol_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];
    }
}