예제 #1
0
/**
 * @brief Handler to interprete Command inputs in regards to arming/disarming
 * @input: ManualControlCommand, AccessoryDesired
 * @output: FlightStatus.Arming
 */
void armHandler(bool newinit)
{
    static ArmState_t armState;

    if (newinit) {
        AccessoryDesiredInitialize();
        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
        armState = ARM_STATE_DISARMED;
    }

    portTickType sysTime = xTaskGetTickCount();

    FlightModeSettingsData settings;
    FlightModeSettingsGet(&settings);
    ManualControlCommandData cmd;
    ManualControlCommandGet(&cmd);
    AccessoryDesiredData acc;

    bool lowThrottle = cmd.Throttle < 0;

    bool armSwitch   = false;

    switch (settings.Arming) {
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
        AccessoryDesiredInstGet(0, &acc);
        armSwitch = true;
        break;
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
        AccessoryDesiredInstGet(1, &acc);
        armSwitch = true;
        break;
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
        AccessoryDesiredInstGet(2, &acc);
        armSwitch = true;
        break;
    default:
        break;
    }

    // immediate disarm on switch
    if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) {
        lowThrottle = true;
    }

    if (forcedDisArm()) {
        // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
        return;
    }

    if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
        // In this configuration we always disarm
        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
        return;
    }


    // The throttle is not low, in case we where arming or disarming, abort
    if (!lowThrottle) {
        switch (armState) {
        case ARM_STATE_DISARMING_MANUAL:
        case ARM_STATE_DISARMING_TIMEOUT:
            armState = ARM_STATE_ARMED;
            break;
        case ARM_STATE_ARMING_MANUAL:
            armState = ARM_STATE_DISARMED;
            break;
        default:
            // Nothing needs to be done in the other states
            break;
        }
        return;
    }

    // The rest of these cases throttle is low
    if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) {
        // In this configuration, we go into armed state as soon as the throttle is low, never disarm
        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
        return;
    }

    // When the configuration is not "Always armed" and no "Always disarmed",
    // the state will not be changed when the throttle is not low
    static portTickType armedDisarmStart;
    float armingInputLevel = 0;

    // Calc channel see assumptions7
    switch (settings.Arming) {
    case FLIGHTMODESETTINGS_ARMING_ROLLLEFT:
        armingInputLevel = 1.0f * cmd.Roll;
        break;
    case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT:
        armingInputLevel = -1.0f * cmd.Roll;
        break;
    case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD:
        armingInputLevel = 1.0f * cmd.Pitch;
        break;
    case FLIGHTMODESETTINGS_ARMING_PITCHAFT:
        armingInputLevel = -1.0f * cmd.Pitch;
        break;
    case FLIGHTMODESETTINGS_ARMING_YAWLEFT:
        armingInputLevel = 1.0f * cmd.Yaw;
        break;
    case FLIGHTMODESETTINGS_ARMING_YAWRIGHT:
        armingInputLevel = -1.0f * cmd.Yaw;
        break;
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY0:
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY1:
    case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
        armingInputLevel = -1.0f * acc.AccessoryVal;
        break;
    }

    bool manualArm    = false;
    bool manualDisarm = false;

    if (armingInputLevel <= -ARMED_THRESHOLD) {
        manualArm = true;
    } else if (armingInputLevel >= +ARMED_THRESHOLD) {
        manualDisarm = true;
    }

    switch (armState) {
    case ARM_STATE_DISARMED:
        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);

        // only allow arming if it's OK too
        if (manualArm && okToArm()) {
            armedDisarmStart = sysTime;
            armState = ARM_STATE_ARMING_MANUAL;
        }
        break;

    case ARM_STATE_ARMING_MANUAL:
        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);

        if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) {
            armState = ARM_STATE_ARMED;
        } else if (!manualArm) {
            armState = ARM_STATE_DISARMED;
        }
        break;

    case ARM_STATE_ARMED:
        // When we get here, the throttle is low,
        // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
        armedDisarmStart = sysTime;
        armState = ARM_STATE_DISARMING_TIMEOUT;
        setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
        break;

    case ARM_STATE_DISARMING_TIMEOUT:
    {
        // we should never reach the disarming timeout if the pathfollower is engaged - reset timeout
        FlightStatusControlChainData cc;
        FlightStatusControlChainGet(&cc);
        if (cc.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
            armedDisarmStart = sysTime;
        }
    }

        // We get here when armed while throttle low, even when the arming timeout is not enabled
        if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) {
            armState = ARM_STATE_DISARMED;
        }

        // Switch to disarming due to manual control when needed
        if (manualDisarm) {
            armedDisarmStart = sysTime;
            armState = ARM_STATE_DISARMING_MANUAL;
        }
        break;

    case ARM_STATE_DISARMING_MANUAL:
        // arming switch disarms immediately,
        if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) {
            armState = ARM_STATE_DISARMED;
        } else if (!manualDisarm) {
            armState = ARM_STATE_ARMED;
        }
        break;
    } // End Switch
}
예제 #2
0
/**
 * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
 * @input: ManualControlCommand
 * @output: StabilizationDesired
 */
void stabilizedHandler(bool newinit)
{
    if (newinit) {
        StabilizationDesiredInitialize();
        StabilizationBankInitialize();
    }
    ManualControlCommandData cmd;
    ManualControlCommandGet(&cmd);

    FlightModeSettingsData settings;
    FlightModeSettingsGet(&settings);

    StabilizationDesiredData stabilization;
    StabilizationDesiredGet(&stabilization);

    StabilizationBankData stabSettings;
    StabilizationBankGet(&stabSettings);

    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
    uint8_t *stab_settings;
    FlightStatusData flightStatus;
    FlightStatusGet(&flightStatus);

    switch (flightStatus.FlightMode) {
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
        break;
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
        stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
        break;
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
        stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
        break;
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
        stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
        break;
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
        stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
        break;
    case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
        stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
        break;
    default:
        // Major error, this should not occur because only enter this block when one of these is true
        AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
        stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
        return;
    }

    stabilization.Roll =
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
        (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
        0; // this is an invalid mode

    stabilization.Pitch =
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
        (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
        0; // this is an invalid mode

    // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
    stabilization.StabilizationMode.Roll  = stab_settings[0];
    stabilization.StabilizationMode.Pitch = stab_settings[1];
    // Other axes (yaw) cannot be Rattitude, so use Rate
    // Should really do this for Attitude mode as well?
    if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
        stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
        stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
    } else {
        stabilization.StabilizationMode.Yaw = stab_settings[2];
        stabilization.Yaw =
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
            (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
            0; // this is an invalid mode
    }

    stabilization.StabilizationMode.Thrust = stab_settings[3];
    stabilization.Thrust = cmd.Thrust;
    StabilizationDesiredSet(&stabilization);
}