Ejemplo n.º 1
0
static void servoMixer(void)
{
    int16_t input[INPUT_ITEMS];
    static int16_t currentOutput[MAX_SERVO_RULES];
    uint8_t i;

    if (f.PASSTHRU_MODE) {
        // Direct passthru from RX
        input[INPUT_ROLL] = rcCommand[ROLL];
        input[INPUT_PITCH] = rcCommand[PITCH];
        input[INPUT_YAW] = rcCommand[YAW];
    } else {
        // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
        input[INPUT_ROLL] = axisPID[ROLL];
        input[INPUT_PITCH] = axisPID[PITCH];
        input[INPUT_YAW] = axisPID[YAW];
    }

    input[INPUT_THROTTLE] = motor[0];
    input[INPUT_AUX1] = mcfg.midrc - rcData[AUX1];
    input[INPUT_AUX2] = mcfg.midrc - rcData[AUX2];
    input[INPUT_AUX3] = mcfg.midrc - rcData[AUX3];
    input[INPUT_AUX4] = mcfg.midrc - rcData[AUX4];
    input[INPUT_RC_ROLL] = mcfg.midrc - rcData[ROLL];
    input[INPUT_RC_PITCH] = mcfg.midrc - rcData[PITCH];
    input[INPUT_RC_YAW] = mcfg.midrc - rcData[YAW];
    input[INPUT_RC_THROTTLE] = mcfg.midrc - rcData[THROTTLE];

    for (i = 0; i < MAX_SERVOS; i++)
        servo[i] = servoMiddle(i);

    // mix servos according to rules
    for (i = 0; i < numberRules; i++) {
        // consider rule if no box assigned or box is active
        if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1 + currentServoMixer[i].box - 1]) {
            uint8_t target = currentServoMixer[i].targetChannel;
            uint8_t from = currentServoMixer[i].fromChannel;
            uint16_t servo_width = cfg.servoConf[target].max - cfg.servoConf[target].min;
            int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
            int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;

            if (currentServoMixer[i].speed == 0)
                currentOutput[i] = input[from];
            else {
                if (currentOutput[i] < input[from])
                    currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
                else if (currentOutput[i] > input[from])
                    currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
            }

            servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
        } else
            currentOutput[i] = 0;
    }

    // servo rates
    for (i = 0; i < MAX_SERVOS; i++)
        servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100;
}
Ejemplo n.º 2
0
static void resetServos(void)
{
    int i;

    // reset all servos to their middle value
    for (i = 0; i < MAX_SERVOS; i++)
        servo[i] = servoMiddle(i);
}
Ejemplo n.º 3
0
void mixTable(void)
{
    int16_t maxMotor;
    uint32_t i;

    if (numberMotor > 3) {
        // prevent "yaw jump" during yaw correction
        axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
    }

    // motors for non-servo mixes
    if (numberMotor > 1)
	{
        for (i = 0; i < numberMotor; i++)
		{
            motor[i] = 
				rcCommand[THROTTLE] * currentMixer[i].throttle + 
				axisPID[PITCH] * currentMixer[i].pitch + 
				axisPID[ROLL] * currentMixer[i].roll + 
				-cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
		}
	}

	
    if (f.FIXED_WING) {
        if (!f.ARMED)
            motor[0] = mcfg.mincommand; // Kill throttle when disarmed
        else
            motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
    }

    // reset all servos
    if (core.useServo)
        resetServos();

    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) {
        // set servo output for gimbal type
        servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
        servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
    }

    // set camstab servo output before applying servo mixer rules
    if (feature(FEATURE_SERVO_TILT)) {
        // vary either pitch or roll by RC channel
        if (rcOptions[BOXCAMSTAB]) {
            if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
                servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
                servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
            } else {
                servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
                servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL]  / 50;
            }
        }
    }

    // run the servo mixer if necessary
    if (core.useServo)
        servoMixer();

    // constrain servos
    for (i = 0; i < MAX_SERVOS; i++)
        servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values

    maxMotor = motor[0];
    for (i = 1; i < numberMotor; i++)
        if (motor[i] > maxMotor)
            maxMotor = motor[i];
		
    for (i = 0; i < numberMotor; i++) 
	{
        if (maxMotor > mcfg.maxthrottle && !f.FIXED_WING)     // this is a way to still have good gyro corrections if at least one motor reaches its max.
            motor[i] -= maxMotor - mcfg.maxthrottle;

        if (feature(FEATURE_3D)) 
		{
            if ((rcData[THROTTLE]) > mcfg.midrc) 
			{
                motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle);
                if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
                    servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max);
                }
            } 
			else 
			{
                motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low);
                if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
                    servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min);
                }
            }
        } 
		else 
		{
            motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
            if ((rcData[THROTTLE]) < mcfg.mincheck) 
			{
                if (!feature(FEATURE_MOTOR_STOP))
                    motor[i] = mcfg.minthrottle;
                else {
                    motor[i] = mcfg.mincommand;
                    f.MOTORS_STOPPED = 1;
                }
            } 
			else 
			{
                f.MOTORS_STOPPED = 0;
            }
        }
		
        if (!f.ARMED) {
            motor[i] = motor_disarmed[i];
        }
    }
}
Ejemplo n.º 4
0
static void servoMixer(void)
{
    int16_t input[INPUT_ITEMS];
    static int16_t currentOutput[MAX_SERVO_RULES];
    uint8_t i;

    if (f.PASSTHRU_MODE) {
        // Direct passthru from RX
        input[INPUT_ROLL] = rcCommand[ROLL];
        input[INPUT_PITCH] = rcCommand[PITCH];
        input[INPUT_YAW] = rcCommand[YAW];
    } else {
        // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
        input[INPUT_ROLL] = axisPID[ROLL];
        input[INPUT_PITCH] = axisPID[PITCH];
        input[INPUT_YAW] = axisPID[YAW];
    }

    input[INPUT_THROTTLE] = motor[0];

    // center the RC input value around the RC middle value
    // by subtracting the RC middle value from the RC input value, we get:
    // data - middle = input
    // 2000 - 1500 = +500
    // 1500 - 1500 = 0
    // 1000 - 1500 = -500
    input[INPUT_AUX1] = rcData[AUX1] - mcfg.midrc;
    input[INPUT_AUX2] = rcData[AUX2] - mcfg.midrc;
    input[INPUT_AUX3] = rcData[AUX3] - mcfg.midrc;
    input[INPUT_AUX4] = rcData[AUX4] - mcfg.midrc;
    input[INPUT_RC_ROLL] = rcData[ROLL] - mcfg.midrc;
    input[INPUT_RC_PITCH] = rcData[PITCH] - mcfg.midrc;
    input[INPUT_RC_YAW] = rcData[YAW] - mcfg.midrc;
    input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - mcfg.midrc;

    // mix servos according to rules
    for (i = 0; i < numberRules; i++) {
        // consider rule if no box assigned or box is active
        if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1 + currentServoMixer[i].box - 1]) {
            uint8_t target = currentServoMixer[i].targetChannel;
            uint8_t from = currentServoMixer[i].fromChannel;
            uint16_t servo_width = cfg.servoConf[target].max - cfg.servoConf[target].min;
            int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
            int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;

            if (currentServoMixer[i].speed == 0) {
                // directly use the input value if speed is not provided
                currentOutput[i] = input[from];
            } else {
                // apply speed constraints
                if (currentOutput[i] < input[from])
                    currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
                else if (currentOutput[i] > input[from])
                    currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
            }

            // start with the output value
            servo[target] = (int16_t)currentOutput[i];

            // apply rate from mixer rule
            servo[target] *= ((float)currentServoMixer[i].rate / 100);

            // apply rate fro m servoconfiguration
            servo[target] *= ((float)cfg.servoConf[target].rate / 100);

            // constrain the width of the servo's movement
            servo[target] = constrain(servo[target], min, max);

            // reverse direction if necessary
            servo[target] *= servoDirection(target, from);

            // center the servo around its middle
            servo[target] += servoMiddle(i);
        }
    }
}
Ejemplo n.º 5
0
void mixTable(void)
{
    int16_t maxMotor;
    uint32_t i;

    if (numberMotor > 3) {
        // prevent "yaw jump" during yaw correction
        axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
    }

    // motors for non-servo mixes
    if (numberMotor > 1)
        for (i = 0; i < numberMotor; i++)
            motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;

    if (f.FIXED_WING) {
        if (!f.ARMED)
            motor[0] = mcfg.mincommand; // Kill throttle when disarmed
        else
            motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
    }

    // airplane / servo mixes
    switch (mcfg.mixerConfiguration) {
        case MULTITYPE_CUSTOM_PLANE:
        case MULTITYPE_FLYING_WING:
        case MULTITYPE_AIRPLANE:
        case MULTITYPE_BI:
        case MULTITYPE_TRI:
        case MULTITYPE_DUALCOPTER:
        case MULTITYPE_SINGLECOPTER:
            servoMixer();
            break;
        case MULTITYPE_GIMBAL:
            servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0);
            servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1);
            break;
    }

    // do camstab
    if (feature(FEATURE_SERVO_TILT)) {
        // center at fixed position, or vary either pitch or roll by RC channel
        servo[0] = servoMiddle(0);
        servo[1] = servoMiddle(1);

        if (rcOptions[BOXCAMSTAB]) {
            if (cfg.gimbal_flags & GIMBAL_MIXTILT) {
                servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
                servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;
            } else {
                servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
                servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL]  / 50;
            }
        }
    }

    // constrain servos
    for (i = 0; i < MAX_SERVOS; i++)
        servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values

    // forward AUX1-4 to servo outputs (not constrained)
    if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
        int offset = core.numServos - 4;
        // offset servos based off number already used in mixer types
        // airplane and servo_tilt together can't be used
        // calculate offset by taking 4 from core.numServos
        for (i = 0; i < 4; i++)
            pwmWriteServo(i + offset, rcData[AUX1 + i]);
    }

    maxMotor = motor[0];
    for (i = 1; i < numberMotor; i++)
        if (motor[i] > maxMotor)
            maxMotor = motor[i];
    for (i = 0; i < numberMotor; i++) {
        if (maxMotor > mcfg.maxthrottle)     // this is a way to still have good gyro corrections if at least one motor reaches its max.
            motor[i] -= maxMotor - mcfg.maxthrottle;

        if (feature(FEATURE_3D)) {
            if ((rcData[THROTTLE]) > mcfg.midrc) {
                motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle);
                if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
                    servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max);
                }
            } else {
                motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low);
                if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) {
                    servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min);
                }
            }
        } else {
            motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle);
            if ((rcData[THROTTLE]) < mcfg.mincheck) {
                if (!feature(FEATURE_MOTOR_STOP))
                    motor[i] = mcfg.minthrottle;
                else {
                    motor[i] = mcfg.mincommand;
                    f.MOTORS_STOPPED = 1;
                }
            } else {
                f.MOTORS_STOPPED = 0;
            }
        }
        if (!f.ARMED) {
            motor[i] = motor_disarmed[i];
        }
    }
}