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; |
} |
} |
|