vtol_att_control_main.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2013-2019 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_att_control_main.cpp
 * Implementation of an attitude controller for VTOL airframes. This module receives data
 * from both the fixed wing- and the multicopter attitude controllers and processes it.
 * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
 * flight or transition). It also publishes the resulting controls on the actuator controls topics.
 *
 * @author Roman Bapst      <bapstr@ethz.ch>
 * @author Lorenz Meier     <lm@inf.ethz.ch>
 * @author Thomas Gubler    <thomasgubler@gmail.com>
 * @author David Vorsin     <davidvorsin@gmail.com>
 * @author Sander Smeets    <sander@droneslab.com>
 * @author Andreas Antener  <andreas@uaventure.com>
 *
 */
#include "vtol_att_control_main.h"
#include <systemlib/mavlink_log.h>
#include <uORB/PublicationQueued.hpp>

using namespace matrix;

VtolAttitudeControl::VtolAttitudeControl() :
    WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
    _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
{
    _vtol_vehicle_status.vtol_in_rw_mode = true;    /* start vtol in rotary wing mode*/

    _params.idle_pwm_mc = PWM_DEFAULT_MIN;
    _params.vtol_motor_id = 0;

    _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
    _params_handles.vtol_motor_id = param_find("VT_MOT_ID");
    _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
    _params_handles.vtol_type = param_find("VT_TYPE");
    _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
    _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
    _params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
    _params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
    _params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
    _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
    _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");

    _params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
    _params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
    _params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
    _params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
    _params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
    _params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
    _params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
    _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
    _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
    _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
    _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
    _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");

    /* fetch initial parameter values */
    parameters_update();

    if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
        _vtol_type = new Tailsitter(this);

    } else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
        _vtol_type = new Tiltrotor(this);

    } else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
        _vtol_type = new Standard(this);

    } else {
        exit_and_cleanup();
    }
}

VtolAttitudeControl::~VtolAttitudeControl()
{
    perf_free(_loop_perf);
}

bool
VtolAttitudeControl::init()
{
    if (!_actuator_inputs_mc.registerCallback()) {
        PX4_ERR("MC actuator controls callback registration failed!");
        return false;
    }

    if (!_actuator_inputs_fw.registerCallback()) {
        PX4_ERR("FW actuator controls callback registration failed!");
        return false;
    }

    return true;
}

/**
* Check for command updates.
*/
void
VtolAttitudeControl::vehicle_cmd_poll()
{
    if (_vehicle_cmd_sub.updated()) {
        _vehicle_cmd_sub.copy(&_vehicle_cmd);
        handle_command();
    }
}

/**
* Check received command
*/
void
VtolAttitudeControl::handle_command()
{
    // update transition command if necessary
    if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
        _transition_command = int(_vehicle_cmd.param1 + 0.5f);

        // Report that we have received the command no matter what we actually do with it.
        // This might not be optimal but is better than no response at all.

        if (_vehicle_cmd.from_external) {
            vehicle_command_ack_s command_ack{};
            command_ack.timestamp = hrt_absolute_time();
            command_ack.command = _vehicle_cmd.command;
            command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
            command_ack.target_system = _vehicle_cmd.source_system;
            command_ack.target_component = _vehicle_cmd.source_component;

            uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
            command_ack_pub.publish(command_ack);
        }
    }
}

/*
 * Returns true if fixed-wing mode is requested.
 * Changed either via switch or via command.
 */
bool
VtolAttitudeControl::is_fixed_wing_requested()
{
    bool to_fw = false;

    if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
        _v_control_mode.flag_control_manual_enabled) {
        to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);

    } else {
        // listen to transition commands if not in manual or mode switch is not mapped
        to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
    }

    // handle abort request
    if (_abort_front_transition) {
        if (to_fw) {
            to_fw = false;

        } else {
            // the state changed to mc mode, reset the abort request
            _abort_front_transition = false;
            _vtol_vehicle_status.vtol_transition_failsafe = false;
        }
    }

    return to_fw;
}

void
VtolAttitudeControl::abort_front_transition(const char *reason)
{
    if (!_abort_front_transition) {
        mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason);
        _abort_front_transition = true;
        _vtol_vehicle_status.vtol_transition_failsafe = true;
    }
}

int
VtolAttitudeControl::parameters_update()
{
    float v;
    int32_t l;
    /* idle pwm for mc mode */
    param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);

    /* vtol motor count */
    param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);

    /* vtol fw permanent stabilization */
    param_get(_params_handles.vtol_fw_permanent_stab, &l);
    _vtol_vehicle_status.fw_permanent_stab = (l == 1);

    param_get(_params_handles.vtol_type, &l);
    _params.vtol_type = l;

    /* vtol lock elevons in multicopter */
    param_get(_params_handles.elevons_mc_lock, &l);
    _params.elevons_mc_lock = (l == 1);

    /* minimum relative altitude for FW mode (QuadChute) */
    param_get(_params_handles.fw_min_alt, &v);
    _params.fw_min_alt = v;

    /* maximum negative altitude error for FW mode (Adaptive QuadChute) */
    param_get(_params_handles.fw_alt_err, &v);
    _params.fw_alt_err = v;

    /* maximum pitch angle (QuadChute) */
    param_get(_params_handles.fw_qc_max_pitch, &l);
    _params.fw_qc_max_pitch = l;

    /* maximum roll angle (QuadChute) */
    param_get(_params_handles.fw_qc_max_roll, &l);
    _params.fw_qc_max_roll = l;

    param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);

    param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);

    /*
     * Minimum transition time can be maximum 90 percent of the open loop transition time,
     * anything else makes no sense and can potentially lead to numerical problems.
     */
    _params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f,
                       _params.front_trans_time_min);


    param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
    param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
    param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
    param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
    param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
    param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
    param_get(_params_handles.airspeed_mode, &l);
    _params.airspeed_disabled = l != 0;
    param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
    param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
    param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
    param_get(_params_handles.diff_thrust, &_params.diff_thrust);

    param_get(_params_handles.diff_thrust_scale, &v);
    _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);

    // make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
    _params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);

    // update the parameters of the instances of base VtolType
    if (_vtol_type != nullptr) {
        _vtol_type->parameters_update();
    }

    return OK;
}

void
VtolAttitudeControl::Run()
{
    if (should_exit()) {
        _actuator_inputs_fw.unregisterCallback();
        _actuator_inputs_mc.unregisterCallback();
        exit_and_cleanup();
        return;
    }

    if (!_initialized) {
        parameters_update();  // initialize parameter cache

        if (_vtol_type->init()) {
            _initialized = true;

        } else {
            exit_and_cleanup();
            return;
        }
    }

    perf_begin(_loop_perf);

    const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in);
    const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in);

    // run on actuator publications corresponding to VTOL mode
    bool should_run = false;

    switch (_vtol_type->get_mode()) {
    case mode::TRANSITION_TO_FW:
    case mode::TRANSITION_TO_MC:
        should_run = updated_fw_in || updated_mc_in;
        break;

    case mode::ROTARY_WING:
        should_run = updated_mc_in;
        break;

    case mode::FIXED_WING:
        should_run = updated_fw_in;
        break;
    }

    if (should_run) {
        // check for parameter updates
        if (_parameter_update_sub.updated()) {
            // clear update
            parameter_update_s pupdate;
            _parameter_update_sub.copy(&pupdate);

            // update parameters from storage
            parameters_update();
        }

        _v_control_mode_sub.update(&_v_control_mode);
        _manual_control_sp_sub.update(&_manual_control_sp);
        _v_att_sub.update(&_v_att);
        _local_pos_sub.update(&_local_pos);
        _local_pos_sp_sub.update(&_local_pos_sp);
        _pos_sp_triplet_sub.update(&_pos_sp_triplet);
        _airspeed_validated_sub.update(&_airspeed_validated);
        _tecs_status_sub.update(&_tecs_status);
        _land_detected_sub.update(&_land_detected);
        vehicle_cmd_poll();

        // check if mc and fw sp were updated
        bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
        bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

        // update the vtol state machine which decides which mode we are in
        if (mc_att_sp_updated || fw_att_sp_updated) {
            _vtol_type->update_vtol_state();

            // reset transition command if not auto control
            if (_v_control_mode.flag_control_manual_enabled) {
                if (_vtol_type->get_mode() == mode::ROTARY_WING) {
                    _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

                } else if (_vtol_type->get_mode() == mode::FIXED_WING) {
                    _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;

                } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
                    /* We want to make sure that a mode change (manual>auto) during the back transition
                     * doesn't result in an unsafe state. This prevents the instant fall back to
                     * fixed-wing on the switch from manual to auto */
                    _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
                }
            }
        }



        // check in which mode we are in and call mode specific functions
        switch (_vtol_type->get_mode()) {
        case mode::TRANSITION_TO_FW:
        case mode::TRANSITION_TO_MC:
            // vehicle is doing a transition
            _vtol_vehicle_status.vtol_in_trans_mode = true;
            _vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition
            _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW);

            _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

            if (mc_att_sp_updated || fw_att_sp_updated) {
                _vtol_type->update_transition_state();
                _v_att_sp_pub.publish(_v_att_sp);
            }

            break;

        case mode::ROTARY_WING:
            // vehicle is in rotary wing mode
            _vtol_vehicle_status.vtol_in_rw_mode = true;
            _vtol_vehicle_status.vtol_in_trans_mode = false;
            _vtol_vehicle_status.in_transition_to_fw = false;

            _vtol_type->update_mc_state();
            _v_att_sp_pub.publish(_v_att_sp);

            break;

        case mode::FIXED_WING:
            // vehicle is in fw mode
            _vtol_vehicle_status.vtol_in_rw_mode = false;
            _vtol_vehicle_status.vtol_in_trans_mode = false;
            _vtol_vehicle_status.in_transition_to_fw = false;

            if (fw_att_sp_updated) {
                _vtol_type->update_fw_state();
                _v_att_sp_pub.publish(_v_att_sp);
            }

            break;
        }

        _vtol_type->fill_actuator_outputs();
        _actuators_0_pub.publish(_actuators_out_0);
        _actuators_1_pub.publish(_actuators_out_1);

        // Advertise/Publish vtol vehicle status
        _vtol_vehicle_status.timestamp = hrt_absolute_time();
        _vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
    }

    perf_end(_loop_perf);
}

int
VtolAttitudeControl::task_spawn(int argc, char *argv[])
{
    VtolAttitudeControl *instance = new VtolAttitudeControl();

    if (instance) {
        _object.store(instance);
        _task_id = task_id_is_work_queue;

        if (instance->init()) {
            return PX4_OK;
        }

    } else {
        PX4_ERR("alloc failed");
    }

    delete instance;
    _object.store(nullptr);
    _task_id = -1;

    return PX4_ERROR;
}

int
VtolAttitudeControl::custom_command(int argc, char *argv[])
{
    return print_usage("unknown command");
}

int
VtolAttitudeControl::print_status()
{
    PX4_INFO("Running");
    perf_print_counter(_loop_perf);
    return PX4_OK;
}

int
VtolAttitudeControl::print_usage(const char *reason)
{
    if (reason) {
        PX4_WARN("%s\n", reason);
    }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
fw_att_control is the fixed wing attitude controller.
)DESCR_STR");

    PRINT_MODULE_USAGE_COMMAND("start");
    PRINT_MODULE_USAGE_NAME("vtol_att_control", "controller");
    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

    return 0;
}

int vtol_att_control_main(int argc, char *argv[])
{
    return VtolAttitudeControl::main(argc, argv);
}