vtol_type.h

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 vtol_type.h
*
* @author Roman Bapst       <bapstroman@gmail.com>
* @author Sander Smeets     <sander@droneslab.com>
* @author Andreas Antener   <andreas@uaventure.com>
*
*/

#ifndef VTOL_TYPE_H
#define VTOL_TYPE_H

#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>

struct Params {
    int32_t idle_pwm_mc;            // pwm value for idle in mc mode
    int32_t vtol_motor_id;
    int32_t vtol_type;
    bool elevons_mc_lock;       // lock elevons in multicopter mode
    float fw_min_alt;           // minimum relative altitude for FW mode (QuadChute)
    float fw_alt_err;           // maximum negative altitude error for FW mode (Adaptive QuadChute)
    float fw_qc_max_pitch;      // maximum pitch angle FW mode (QuadChute)
    float fw_qc_max_roll;       // maximum roll angle FW mode (QuadChute)
    float front_trans_time_openloop;
    float front_trans_time_min;
    float front_trans_duration;
    float back_trans_duration;
    float transition_airspeed;
    float front_trans_throttle;
    float back_trans_throttle;
    float airspeed_blend;
    bool airspeed_disabled;
    float front_trans_timeout;
    float mpc_xy_cruise;
    int32_t fw_motors_off;          /**< bitmask of all motors that should be off in fixed wing mode */
    int32_t diff_thrust;
    float diff_thrust_scale;
};

// Has to match 1:1 msg/vtol_vehicle_status.msg
enum class mode {
    TRANSITION_TO_FW = 1,
    TRANSITION_TO_MC = 2,
    ROTARY_WING = 3,
    FIXED_WING = 4
};

enum class vtol_type {
    TAILSITTER = 0,
    TILTROTOR,
    STANDARD
};

// these are states that can be applied to a selection of multirotor motors.
// e.g. if we need to shut off some motors after transitioning to fixed wing mode
// we can individually disable them while others might still need to be enabled to produce thrust.
// we can select the target motors via VT_FW_MOT_OFFID
enum class motor_state {
    ENABLED = 0,        // motor max pwm will be set to the standard max pwm value
    DISABLED,           // motor max pwm will be set to a value that shuts the motor off
    IDLE,               // motor max pwm will be set to VT_IDLE_PWM_MC
    VALUE               // motor max pwm will be set to a specific value provided, see set_motor_state()
};

/**
 * @brief      Used to specify if min or max pwm values should be altered
 */
enum class pwm_limit_type {
    TYPE_MINIMUM = 0,
    TYPE_MAXIMUM
};

class VtolAttitudeControl;

class VtolType
{
public:

    VtolType(VtolAttitudeControl *att_controller);
    VtolType(const VtolType &) = delete;
    VtolType &operator=(const VtolType &) = delete;

    virtual ~VtolType() = default;

    /**
     * Initialise.
     */
    bool init();

    /**
     * Update vtol state.
     */
    virtual void update_vtol_state() = 0;

    /**
     * Update transition state.
     */
    virtual void update_transition_state() = 0;

    /**
     * Update multicopter state.
     */
    virtual void update_mc_state();

    /**
     * Update fixed wing state.
     */
    virtual void update_fw_state();

    /**
     * Write control values to actuator output topics.
     */
    virtual void fill_actuator_outputs() = 0;

    /**
     * Special handling opportunity for the time right after transition to FW
     * before TECS is running.
     */
    virtual void waiting_on_tecs() {}

    /**
     * Checks for fixed-wing failsafe condition and issues abort request if needed.
     */
    void check_quadchute_condition();

    /**
     * Returns true if we're allowed to do a mode transition on the ground.
     */
    bool can_transition_on_ground();



    mode get_mode() {return _vtol_mode;}

    virtual void parameters_update() = 0;

protected:
    VtolAttitudeControl *_attc;
    mode _vtol_mode;

    static constexpr const int num_outputs_max = 8;

    struct vehicle_attitude_s       *_v_att;                //vehicle attitude
    struct vehicle_attitude_setpoint_s  *_v_att_sp;         //vehicle attitude setpoint
    struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint
    struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint
    struct vehicle_control_mode_s       *_v_control_mode;   //vehicle control mode
    struct vtol_vehicle_status_s        *_vtol_vehicle_status;
    struct actuator_controls_s          *_actuators_out_0;          //actuator controls going to the mc mixer
    struct actuator_controls_s          *_actuators_out_1;          //actuator controls going to the fw mixer (used for elevons)
    struct actuator_controls_s          *_actuators_mc_in;          //actuator controls from mc_rate_control
    struct actuator_controls_s          *_actuators_fw_in;          //actuator controls from fw_att_control
    struct vehicle_local_position_s         *_local_pos;
    struct vehicle_local_position_setpoint_s    *_local_pos_sp;
    struct airspeed_validated_s                 *_airspeed_validated;                   // airspeed
    struct tecs_status_s                *_tecs_status;
    struct vehicle_land_detected_s          *_land_detected;

    struct Params                   *_params;

    bool flag_idle_mc = false;      //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"

    bool _pusher_active = false;
    float _mc_roll_weight = 1.0f;   // weight for multicopter attitude controller roll output
    float _mc_pitch_weight = 1.0f;  // weight for multicopter attitude controller pitch output
    float _mc_yaw_weight = 1.0f;    // weight for multicopter attitude controller yaw output
    float _mc_throttle_weight = 1.0f;   // weight for multicopter throttle command. Used to avoid

    // motors spinning up or cutting too fast when doing transitions.
    float _thrust_transition = 0.0f;    // thrust value applied during a front transition (tailsitter & tiltrotor only)

    float _ra_hrate = 0.0f;         // rolling average on height rate for quadchute condition
    float _ra_hrate_sp = 0.0f;      // rolling average on height rate setpoint for quadchute condition

    bool _flag_was_in_trans_mode = false;   // true if mode has just switched to transition

    hrt_abstime _trans_finished_ts = 0;

    bool _tecs_running = false;
    hrt_abstime _tecs_running_ts = 0;

    motor_state _motor_state = motor_state::DISABLED;



    /**
     * @brief      Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures
     *             that they are spinning in mc mode.
     *
     * @return     true on success
     */
    bool set_idle_mc();

    /**
     * @brief      Sets mc motor minimum pwm to PWM_MIN which ensures that the
     *             motors stop spinning on zero throttle in fw mode.
     *
     * @return     true on success
     */
    bool set_idle_fw();


    /**
     * @brief      Sets state of a selection of motors, see struct motor_state
     *
     * @param[in]  current_state  The current motor state
     * @param[in]  next_state     The next state
     * @param[in]  value          Desired pwm value if next_state =
     *                            motor_state::VALUE
     *
     * @return     next_state if succesfull, otherwise current_state
     */
    motor_state set_motor_state(const motor_state current_state, const motor_state next_state, const int value = 0);

private:


    /**
     * @brief      Stores the max pwm values given by the system.
     */
    struct pwm_output_values _min_mc_pwm_values {};
    struct pwm_output_values _max_mc_pwm_values {};
    struct pwm_output_values _disarmed_pwm_values {};

    /**
     * @brief      Adjust minimum/maximum pwm values for the output channels.
     *
     * @param      pwm_output_values  Struct containing the limit values for each channel
     * @param[in]  type               Specifies if min or max limits are adjusted.
     *
     * @return     True on success.
     */
    bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type);

    /**
     * @brief      Determines if channel is set in target.
     *
     * @param[in]  channel  The channel
     * @param[in]  target   The target to check on.
     *
     * @return     True if motor off channel, False otherwise.
     */
    bool is_channel_set(const int channel, const int target);

};

#endif