Esempio n. 1
0
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
    // Called at PID rate

    if (launchState.launchDetected) {
        bool applyLaunchIdleLogic = true;

        if (launchState.motorControlAllowed) {
            // If launch detected we are in launch procedure - control airplane
            const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);

            // If user moves the stick - finish the launch
            if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) {
                launchState.launchFinished = true;
            }

            // Abort launch after a pre-set time
            if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) {
                launchState.launchFinished = true;
            }

            // Motor control enabled
            if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
                // Don't apply idle logic anymore
                applyLaunchIdleLogic = false;

                // Increase throttle gradually over `launch_motor_spinup_time` milliseconds
                if (navConfig()->fw.launch_motor_spinup_time > 0) {
                    const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
                    const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
                    rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
                                                      0.0f, navConfig()->fw.launch_motor_spinup_time,
                                                      minIdleThrottle, navConfig()->fw.launch_throttle);
                }
                else {
                    rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
                }
            }
        }

        if (applyLaunchIdleLogic) {
            // Launch idle logic - low throttle and zero out PIDs
            applyFixedWingLaunchIdleLogic();
        }
    }
    else {
        // We are waiting for launch - update launch detector
        updateFixedWingLaunchDetector(currentTimeUs);

        // Launch idle logic - low throttle and zero out PIDs
        applyFixedWingLaunchIdleLogic();
    }

    // Control beeper
    if (!launchState.launchFinished) {
        beeper(BEEPER_LAUNCH_MODE_ENABLED);
    }

    // Lock out controls
    rcCommand[ROLL] = 0;
    rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
    rcCommand[YAW] = 0;
}
Esempio n. 2
0
File: pid.c Progetto: dan557/inav
float pidRcCommandToRate(int16_t stick, uint8_t rate)
{
    return scaleRangef((float) stick, (float) -500, (float) 500, (float) -rate, (float) rate) * 10;
}
Esempio n. 3
0
File: pid.c Progetto: dan557/inav
int16_t pidAngleToRcCommand(float angleDeciDegrees, int16_t maxInclination)
{
    angleDeciDegrees = constrainf(angleDeciDegrees, (float) -maxInclination, (float) maxInclination);
    return scaleRangef((float) angleDeciDegrees, (float) -maxInclination, (float) maxInclination, -500.0f, 500.0f);
}
Esempio n. 4
0
File: pid.c Progetto: dan557/inav
/*
Map stick positions to desired rotatrion rate in given axis.
Rotation rate in dps at full stick deflection is defined by axis rate measured in dps/10
Rate 20 means 200dps at full stick deflection
*/
float pidRateToRcCommand(float rateDPS, uint8_t rate)
{
    const float rateDPS_10 = constrainf(rateDPS / 10.0f, (float) -rate, (float) rate);
    return scaleRangef(rateDPS_10, (float) -rate, (float) rate, -500.0f, 500.0f);
}
Esempio n. 5
0
File: pid.c Progetto: dan557/inav
static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
{
    stick = constrain(stick, -500, 500);
    return scaleRangef((float) stick, -500.0f, 500.0f, (float) -maxInclination, (float) maxInclination);
}