Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

merged in mwii2.3 generic servo handler. completely untested.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@434 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-10-12 07:03:01 +00:00
parent 37b73a057b
commit 9ebd82c5ef
7 changed files with 83 additions and 119 deletions
Download patch file Download diff file Expand all files Collapse all files

82
src/mixer.c
View file

@ -3,7 +3,7 @@
static uint8_t numberMotor = 0;
int16_t motor[MAX_MOTORS];
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
static motorMixer_t currentMixer[MAX_MOTORS];
@ -130,6 +130,30 @@ const mixer_t mixers[] = {
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
};
int16_t servoMiddle(int nr)
{
// Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than
// the number of RC channels, it means the center value is taken FROM that RC channel (by its index)
if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS)
return rcData[cfg.servoConf[nr].middle];
else
return cfg.servoConf[nr].middle;
}
int servoDirection(int nr, int lr)
{
// servo.rate is overloaded for servos that don't have a rate, but only need direction
// bit set = negative, clear = positive
// rate[2] = ???_direction
// rate[1] = roll_direction
// rate[0] = pitch_direction
// servo.rate is also used as gimbal gain multiplier (yeah)
if (cfg.servoConf[nr].rate & lr)
return -1;
else
return 1;
}
void mixerInit(void)
{
int i;
@ -319,17 +343,17 @@ void mixTable(void)
// airplane / servo mixes
switch (mcfg.mixerConfiguration) {
case MULTITYPE_BI:
servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT
servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT
break;
case MULTITYPE_TRI:
servo[5] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
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;
case MULTITYPE_AIRPLANE:
@ -337,47 +361,47 @@ void mixTable(void)
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (!f.ARMED)
servo[7] = mcfg.mincommand;
else
servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle);
motor[0] = servo[7];
if (f.PASSTHRU_MODE) {
// do not use sensors for correction, simple 2 channel mixing
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc);
servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]);
servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]);
} else {
// use sensors to correct (gyro only or gyro + acc)
servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL];
servo[1] = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL];
servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]);
servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]);
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max);
servo[3] += servoMiddle(3);
servo[4] += servoMiddle(4);
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
uint16_t aux[2] = { 0, 0 };
if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY))
aux[0] = rcData[AUX3] - mcfg.midrc;
if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
aux[1] = rcData[AUX4] - mcfg.midrc;
servo[0] = cfg.gimbal_pitch_mid + aux[0];
servo[1] = cfg.gimbal_roll_mid + aux[1];
// 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] -= (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16;
servo[1] += (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 + cfg.gimbal_roll_gain * angle[ROLL] / 16;
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] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16;
servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;
servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50;
}
}
servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
}
// 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 = 0;
if (feature(FEATURE_SERVO_TILT))