ardupilot/ArduCopter/mode_flip.cpp

#include "Copter.h"

#if MODE_FLIP_ENABLED

/*
 * Flight mode which performs a flip, then self-exits.
 *      original implementation in 2010 by Jose Julio
 *      Adapted and updated for AC2 in 2011 by Jason Short
 *
 *      Flip can be initiated by either a (configured) RC switch or switching modes to FLIP.
 *      With no pilot input, the flip is in the ROLL_LEFT direction.
 *      By gently holding the roll/pitch stick in a direction during mode entry, the pilot can select the flip's direction.
 *      While in FLIP mode, the flip can be aborted by:
 *      - Pushing either the roll or pitch stick to a 'high magnitude' value.
 *      - Toggling the (configured) RC switch away from HIGH.
 *      - Switching mode away from FLIP.
 *      No matter whether the flip completes or is aborted, the mode will switch away afterwards.
 *
 */

#define FLIP_THR_INC            0.20f           // throttle increase during FlipState::Start stage (under 45deg lean angle)
#define FLIP_THR_DEC            0.24f           // throttle decrease during FlipState::Roll stage (between 45deg ~ -90deg roll)
#define FLIP_RECOVERY_ANGLE_RAD radians(5.0)    // consider successful recovery when roll is back within 5 degrees of original

#define FLIP_ROLL_RIGHT      1      // used to set flip_dir
#define FLIP_ROLL_LEFT      -1      // used to set flip_dir

#define FLIP_PITCH_BACK      1      // used to set flip_dir
#define FLIP_PITCH_FORWARD  -1      // used to set flip_dir

#define FLIP_RATE_DEFAULT_DPS 400.0f  // default flip rate in deg/s

const AP_Param::GroupInfo ModeFlip::var_info[] = {

    // @Param: RATE
    // @DisplayName: Flip Mode Rotational Rate
    // @Description: Rotational Rate for Flip Mode in Deg/s.  Be sure to set a rotational rate that the aircraft can achieve.
    // @Units: deg/s
    // @Range: 60 1000
    // @Increment: 1
    // @User: Standard
    AP_GROUPINFO("RATE", 1, ModeFlip, flip_rate_dps, FLIP_RATE_DEFAULT_DPS),

    AP_GROUPEND
};

// constructor
ModeFlip::ModeFlip(void) : Mode()
{
    // load parameter defaults
    AP_Param::setup_object_defaults(this, var_info);
}

// flip_init - initialise flip controller
bool ModeFlip::init(bool ignore_checks)
{
    // only allow flip from some flight modes, for example ACRO, Stabilize, AltHold or FlowHold flight modes
    if (!copter.flightmode->allows_flip()) {
        return false;
    }

    // if in acro or stabilize ensure throttle is above zero
    if (copter.ap.throttle_zero && (copter.flightmode->mode_number() == Mode::Number::ACRO || copter.flightmode->mode_number() == Mode::Number::STABILIZE)) {
        return false;
    }

    if (input_is_high_magnitude(*channel_roll) || input_is_high_magnitude(*channel_pitch)) {
        return false;
    }

    // only allow flip when flying
    if (!motors->armed() || copter.ap.land_complete) {
        return false;
    }

    // capture original flight mode so that we can return to it after completion
    orig_control_mode = copter.flightmode->mode_number();

    // initialise state
    _state = FlipState::Start;
    abandon_requested = false;
    start_time_ms = millis();

    roll_dir = pitch_dir = 0;

    // choose direction based on pilot's roll and pitch sticks
    if (channel_pitch->get_control_in() > 300) {
        pitch_dir = FLIP_PITCH_BACK;
    } else if (channel_pitch->get_control_in() < -300) {
        pitch_dir = FLIP_PITCH_FORWARD;
    } else if (channel_roll->get_control_in() >= 0) {
        roll_dir = FLIP_ROLL_RIGHT;
    } else {
        roll_dir = FLIP_ROLL_LEFT;
    }

    // log start of flip
    LOGGER_WRITE_EVENT(LogEvent::FLIP_START);

    // capture current attitude which will be used during the FlipState::Recovery stage
    const float angle_max_rad = attitude_control->lean_angle_max_rad();
    orig_attitude_euler_rad.x = constrain_float(ahrs.get_roll_rad(), -angle_max_rad, angle_max_rad);
    orig_attitude_euler_rad.y = constrain_float(ahrs.get_pitch_rad(), -angle_max_rad, angle_max_rad);
    orig_attitude_euler_rad.z = ahrs.get_yaw_rad();

    return true;
}

// run - runs the flip controller
// should be called at 100hz or more
void ModeFlip::run()
{
    // get flip rotation rate parameter and constrain to range from 60 to 1000 deg/s.
    const float flip_rate_rads = radians(constrain_float(flip_rate_dps.get(), 60.0f, 1000.0f));
    // determine timeout for flip based on rotation rate.  Time is doubled to allow for poorly tuned vehicles.
    const float flip_time_out_ms = 1000.0f * (2.0f * radians(360.0f) / flip_rate_rads);

    // if pilot inputs roll > 40deg or timeout occurs abandon flip
    if (abandon_requested || !motors->armed() || input_is_high_magnitude(*channel_roll) || input_is_high_magnitude(*channel_pitch) || ((millis() - start_time_ms) > flip_time_out_ms)) {
        _state = FlipState::Abandon;
        abandon_requested = false;
    }

    // get pilot's desired throttle
    float throttle_out = get_pilot_desired_throttle();

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // get corrected angle based on direction and axis of rotation
    // we flip the sign of flip_angle to minimize the code repetition
    float flip_angle_rad;

    if (roll_dir != 0) {
        flip_angle_rad = ahrs.get_roll_rad() * roll_dir;
    } else {
        flip_angle_rad = ahrs.get_pitch_rad() * pitch_dir;
    }

    // state machine
    switch (_state) {

    case FlipState::Start:
        // request flip_rate_rads in the nonzero direction
        attitude_control->input_rate_bf_roll_pitch_yaw_rads(flip_rate_rads * roll_dir, flip_rate_rads * pitch_dir, 0.0);

        // increase throttle
        throttle_out += FLIP_THR_INC;

        // beyond specified lean angle: move to next stage
        if (flip_angle_rad >= radians(45.0)) {
            if (roll_dir != 0) {
                // we are rolling
                _state = FlipState::Roll;
            } else {
                // we are pitching
                _state = FlipState::Pitch_A;
            }
        }
        break;

    case FlipState::Roll:
        // request flip_rate_rads roll
        attitude_control->input_rate_bf_roll_pitch_yaw_rads(flip_rate_rads * roll_dir, 0.0, 0.0);
        // decrease throttle
        throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);

        // if state transition conditions are met: move on to recovery
        if ((flip_angle_rad < radians(45.0)) && (flip_angle_rad > -radians(90.0))) {
            _state = FlipState::Recover;
        }
        break;

    case FlipState::Pitch_A:
        // request flip_rate_rads pitch
        attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0f, flip_rate_rads * pitch_dir, 0.0);
        // decrease throttle
        throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);

        // check roll for inversion
        if ((fabsf(ahrs.get_roll_rad()) > radians(90.0)) && (flip_angle_rad > radians(45.0))) {
            _state = FlipState::Pitch_B;
        }
        break;

    case FlipState::Pitch_B:
        // request flip_rate_rads pitch
        attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0, flip_rate_rads * pitch_dir, 0.0);
        // decrease throttle
        throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);

        // check roll for inversion
        if ((fabsf(ahrs.get_roll_rad()) < radians(90.0)) && (flip_angle_rad > -radians(45.0))) {
            _state = FlipState::Recover;
        }
        break;

    case FlipState::Recover: {
        // use originally captured earth-frame angle targets to recover
        attitude_control->input_euler_angle_roll_pitch_yaw_rad(orig_attitude_euler_rad.x, orig_attitude_euler_rad.y, orig_attitude_euler_rad.z, false);

        // increase throttle to gain any lost altitude
        throttle_out += FLIP_THR_INC;

        float recovery_angle_rad;
        if (roll_dir != 0) {
            // we are rolling
            recovery_angle_rad = fabsf(orig_attitude_euler_rad.x - ahrs.get_roll_rad());
        } else {
            // we are pitching
            recovery_angle_rad = fabsf(orig_attitude_euler_rad.y - ahrs.get_pitch_rad());
        }

        // check for successful recovery
        if (fabsf(recovery_angle_rad) <= FLIP_RECOVERY_ANGLE_RAD) {
            // restore original flight mode
            if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
                // this should never happen but just in case
                copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
            }
            // log successful completion
            LOGGER_WRITE_EVENT(LogEvent::FLIP_END);
        }
        break;

    }
    case FlipState::Abandon:
        // restore original flight mode
        if (!copter.set_mode(orig_control_mode, ModeReason::FLIP_COMPLETE)) {
            // this should never happen but just in case
            copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN);
        }
        // log abandoning flip
        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED);
        break;
    }

    // output pilot's throttle without angle boost
    attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
}

void ModeFlip::abandon_flip()
{
    abandon_requested = true;
}

bool ModeFlip::input_is_high_magnitude(RC_Channel &input) const
{
    return abs(input.get_control_in()) >= 4000;
}

#endif