Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1221 → Rev 1222

/branches/V0.73d Code Redesign killagreg/fc.c
126,18 → 126,19
 
// MK flags
uint16_t ModelIsFlying = 0;
uint8_t MKFlags = 0;
uint8_t volatile MKFlags = 0;
 
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
 
int16_t Ki = 10300 / 33;
 
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 
// setpoints for motors
 
volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;
uint8_t RequiredMotors = 0;
 
 
// stick values derived by rc channels readings
158,7 → 159,7
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
 
 
fc_param_t FCParam = {48,251,16,58,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,70,90,65,64,100};
fc_param_t FCParam = {48,251,16,58,64,8,150,150,2,10,0,0,0,0,0,0,0,0,100,70,90,65,64,100,0,0,0};
 
 
 
529,53 → 530,27
/************************************************************************/
void SendMotorData(void)
{
uint8_t i;
if(!(MKFlags & MKFLAG_MOTOR_RUN))
{
#ifdef USE_QUADRO
Motor1 = 0;
Motor2 = 0;
Motor3 = 0;
Motor4 = 0;
if(MotorTest[0]) Motor1 = MotorTest[0];
if(MotorTest[1]) Motor2 = MotorTest[1];
if(MotorTest[2]) Motor3 = MotorTest[2];
if(MotorTest[3]) Motor4 = MotorTest[3];
#else
Motor1 = 0;
Motor2 = 0;
Motor3 = 0;
Motor4 = 0;
Motor5 = 0;
Motor6 = 0;
Motor7 = 0;
Motor8 = 0;
if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
 
#endif
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
for(i = 0; i < MAX_MOTORS; i++)
{
if(!MotorTest_Active) Motor[i].SetPoint = 0;
else Motor[i].SetPoint = MotorTest[i];
}
if(MotorTest_Active) MotorTest_Active--;
}
#ifdef USE_QUADRO
 
DebugOut.Analog[12] = Motor1; // Front
DebugOut.Analog[13] = Motor2; // Rear
DebugOut.Analog[14] = Motor4; // Left
DebugOut.Analog[15] = Motor3; // Right
#else // OCTO Motor addresses are counted clockwise starting at the head
DebugOut.Analog[12] = (Motor1 + Motor2) / 2;
DebugOut.Analog[13] = (Motor5 + Motor6) / 2;
DebugOut.Analog[14] = (Motor7 + Motor8) / 2;
DebugOut.Analog[15] = (Motor3 + Motor4) / 2;
#endif
DebugOut.Analog[12] = Motor[0].SetPoint; // Front
DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
DebugOut.Analog[14] = Motor[3].SetPoint; // Left
DebugOut.Analog[15] = Motor[2].SetPoint; // Right
//Start I2C Interrupt Mode
twi_state = TWI_STATE_MOTOR_TX;
I2C_Start();
I2C_Start(TWI_STATE_MOTOR_TX);
}
 
 
 
/************************************************************************/
/* Map the parameter to poti values */
/************************************************************************/
652,7 → 627,7
/************************************************************************/
void MotorControl(void)
{
int16_t MotorValue, h, tmp_int;
int16_t h, tmp_int;
 
// Mixer Fractions that are combined for Motor Control
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
672,10 → 647,8
static int8_t TimerDebugOut = 0;
static uint16_t UpdateCompassCourse = 0;
// high resolution motor values for smoothing of PID motor outputs
static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0;
#ifndef USE_QUADRO
static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0;
#endif
static int16_t MotorValue[MAX_MOTORS];
uint8_t i;
 
Mean();
GRN_ON;
962,6 → 935,8
// update gyro control loop factors
GyroPFactor = FCParam.GyroP + 10;
GyroIFactor = FCParam.GyroI;
GyroYawPFactor = FCParam.GyroP + 10;
GyroYawIFactor = FCParam.GyroI;
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1054,7 → 1029,9
StickNick = 0;
StickRoll = 0;
GyroPFactor = 90;
GyroIFactor = 120;
GyroIFactor = 120;
GyroYawPFactor = 90;
GyroYawIFactor = 120;
LoopingRoll = 0;
LoopingNick = 0;
MaxStickNick = 0;
1446,7 → 1423,6
// DebugOut.Analog[25] = GyroRoll/2;
DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
// DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
// DebugOut.Analog[29] = (int16_t)FCParam.KalmanK;
DebugOut.Analog[30] = GPSStickNick;
DebugOut.Analog[31] = GPSStickRoll;
}
1485,13 → 1461,7
}
PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor + (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part
 
// octo has a double yaw momentum because of the doubled motor number
// therefore double D-Part and halfen P-Part for the same result
#ifdef USE_OCTO
PDPartYaw = (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN));
#else
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN));
#endif
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN));
 
//DebugOut.Analog[21] = PDPartNick;
//DebugOut.Analog[22] = PDPartRoll;
1503,6 → 1473,17
CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// all BL-Ctrl connected?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(MissingMotor)
{
// if we are in the lift off condition
if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) )
ModelIsFlying = 1; // keep within lift off condition
GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The height control algorithm reduces the gas but does not increase the gas.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1638,207 → 1619,24
CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
 
#ifdef USE_QUADRO
 
// QuadroKopter Mixer
 
// Motor Front (++)
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
// Motor Rear (-+)
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
// Motor Right (--)
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
// Motor Left (+-)
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
#endif
 
#ifdef USE_OCTO
 
// OctoKopter Mixer
 
// Motor 1 (+++)
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1= MotorValue;
 
// Motor 2 (+--)
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
// Motor 3 (+-+)
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
// Motor 4 (---)
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
// Motor 5 (--+)
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
// Motor 6 (-+-)
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
// Motor7 (-++)
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
// Motor8 (++-)
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
#ifdef USE_OCTO2
 
// Octokopter Mixer alternativ setup
 
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
#ifdef USE_OCTO3
 
// Octokopter Mixer alternativ setup
 
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
MotorValue = MotorValue1 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor1 = MotorValue;
 
MotorValue = GasMixFraction + NickMixFraction - YawMixFraction;
MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
MotorValue = MotorValue2 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor2 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
MotorValue = MotorValue3 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor3 = MotorValue;
 
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
MotorValue = MotorValue4 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor4 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
MotorValue = MotorValue5 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor5 = MotorValue;
 
MotorValue = GasMixFraction - NickMixFraction - YawMixFraction;
MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
MotorValue = MotorValue6 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor6 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
MotorValue = MotorValue7 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor7 = MotorValue;
 
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
MotorValue = MotorValue8 / STICK_GAIN;
CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
Motor8 = MotorValue;
#endif
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for(i = 0; i < MAX_MOTORS; i++)
{
int16_t tmp;
if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer
{
tmp = ((int32_t)GasMixFraction * Mixer.Motor[i][MIX_GAS] ) / 64L;
tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L;
tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
tmp += ((int32_t)YawMixFraction * Mixer.Motor[i][MIX_YAW] ) / 64L;
MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]); // Spike Filter
tmp = MotorValue[i] / STICK_GAIN;
CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
Motor[i].SetPoint = tmp;
}
else Motor[i].SetPoint = 0;
}
}