/**
 * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired
 * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands
 * @output: PathDesired
 */
void pathFollowerHandler(bool newinit)
{
    if (newinit) {
        plan_initialize();
    }

    FlightStatusFlightModeOptions flightMode;
    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
    FlightStatusFlightModeAssistOptions flightModeAssist;
    FlightStatusFlightModeGet(&flightMode);
    FlightStatusFlightModeAssistGet(&flightModeAssist);
    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);

    if (newinit) {
        // After not being in this mode for a while init at current height
        switch (flightMode) {
        case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
            plan_setup_returnToBase();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
            if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
                (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
                // Switch from primary (just entered this PH flight mode) into brake
                plan_setup_assistedcontrol();
            } else {
                plan_setup_positionHold();
            }
            break;
        case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
            plan_setup_CourseLock();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
            plan_setup_VelocityRoam();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
            plan_setup_HomeLeash();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
            plan_setup_AbsolutePosition();
            break;

        case FLIGHTSTATUS_FLIGHTMODE_LAND:
            if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
                plan_setup_land();
            } else {
                plan_setup_VelocityRoam();
            }
            break;
        case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
            plan_setup_AutoTakeoff();
            break;
        case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
            plan_setup_AutoCruise();
            break;

        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
        case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
            if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
                // Just initiated braking after returning from stabi control
                plan_setup_assistedcontrol();
            }
            break;

        default:
            plan_setup_positionHold();
            break;
        }
    }

    switch (flightMode) {
    case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
        plan_run_CourseLock();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
        plan_run_VelocityRoam();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
        plan_run_HomeLeash();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
        plan_run_AbsolutePosition();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_LAND:
        if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
            plan_run_VelocityRoam();
        }
        break;
    case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
        plan_run_AutoTakeoff();
        break;
    case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
        plan_run_AutoCruise();
        break;
    default:
        break;
    }
}
Exemple #2
0
void plan_run_VelocityRoam()
{
    // float alpha;
    PathDesiredData pathDesired;
    FlightStatusAssistedControlStateOptions assistedControlFlightMode;
    FlightStatusFlightModeOptions flightMode;

    PathDesiredGet(&pathDesired);
    FlightModeSettingsPositionHoldOffsetData offset;
    FlightModeSettingsPositionHoldOffsetGet(&offset);
    FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
    FlightStatusFlightModeGet(&flightMode);
    StabilizationBankData stabSettings;
    StabilizationBankGet(&stabSettings);

    ManualControlCommandData cmd;
    ManualControlCommandGet(&cmd);

    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);

    bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);

    if (!flagRollPitchHasInput) {
        // no movement desired, re-enter positionHold at current start-position
        if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
            // initiate braking and change assisted control flight mode to braking
            if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
                // avoid brake then hold sequence to continue descent.
                plan_setup_land_from_velocityroam();
            } else {
                plan_setup_assistedcontrol();
            }
        }
        // otherwise nothing to do in braking/hold modes
    } else {
        PositionStateData positionState;
        PositionStateGet(&positionState);

        // Revert assist control state to primary, which in this case implies
        // we are in roaming state (a GPS vector assisted velocity roam)
        assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;

        // Calculate desired velocity in each direction
        float angle;
        AttitudeStateYawGet(&angle);
        angle = DEG2RAD(angle);
        float cos_angle  = cosf(angle);
        float sine_angle = sinf(angle);
        float rotated[2] = {
            -cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
            -cmd.Pitch * sine_angle + cmd.Roll * cos_angle
        };
        // flip pitch to have pitch down (away) point north
        float horizontalVelMax;
        float verticalVelMax;
        VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
        VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
        float velocity_north = rotated[0] * horizontalVelMax;
        float velocity_east  = rotated[1] * horizontalVelMax;
        float velocity_down  = 0.0f;

        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
            FlightModeSettingsLandingVelocityGet(&velocity_down);
        }

        float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
        velocity = sqrtf(velocity);

        // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
        // expect new stick input
        // if two stick input pilot is fighting wind manually and we use fly by velocity
        // in reality setting velocity desired to zero will fight wind anyway.

        pathDesired.Start.North = positionState.North;
        pathDesired.Start.East  = positionState.East;
        pathDesired.Start.Down  = positionState.Down;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]  = velocity_east;
        pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]  = velocity_down;

        pathDesired.End.North = positionState.North;
        pathDesired.End.East  = positionState.East;
        pathDesired.End.Down  = positionState.Down;

        pathDesired.StartingVelocity = velocity;
        pathDesired.EndingVelocity   = velocity;
        pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
        if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
            pathDesired.Mode = PATHDESIRED_MODE_LAND;
            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
        } else {
            pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
        }
        PathDesiredSet(&pathDesired);
        FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
    }
}