128,7 → 128,11 |
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
|
// setpoints for motors |
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
#ifdef HEXAKOPTER |
volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left; |
#else |
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr |
#endif |
|
// stick values derived by rc channels readings |
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0; |
382,6 → 386,18 |
{ |
if(!(MKFlags & MKFLAG_MOTOR_RUN)) |
{ |
#ifdef HEXAKOPTER |
Motor_RearLeft = 0; |
Motor_FrontLeft = 0; |
Motor_RearRight = 0; |
Motor_FrontRight = 0; |
Motor_Right = 0; |
Motor_Left = 0; |
if(MotorTest[0]) Motor_FrontLeft = Motor_FrontRight = MotorTest[0]; |
if(MotorTest[1]) Motor_RearLeft = Motor_RearRight = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
#else |
Motor_Rear = 0; |
Motor_Front = 0; |
Motor_Right = 0; |
390,13 → 406,10 |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
#endif |
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off |
} |
DebugOut.Analog[12] = Motor_Front; |
DebugOut.Analog[13] = Motor_Rear; |
DebugOut.Analog[14] = Motor_Left; |
DebugOut.Analog[15] = Motor_Right; |
|
|
//Start I2C Interrupt Mode |
twi_state = TWI_STATE_MOTOR_TX; |
I2C_Start(); |
474,7 → 487,7 |
void MotorControl(void) |
{ |
int16_t MotorValue, pd_result, h, tmp_int; |
int16_t YawMixFraction, GasMixFraction; |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
static int32_t SumNick = 0, SumRoll = 0; |
static int32_t SetPointYaw = 0; |
static int32_t IntegralErrorNick = 0; |
575,6 → 588,7 |
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
|
|
// if motors are off and the gas stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) ) |
{ |
694,7 → 708,7 |
{ |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 200; // do not repeat if once executed |
delay_startmotors = 0; // do not repeat if once executed |
Model_Is_Flying = 1; |
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START |
SetPointYaw = 0; |
715,7 → 729,7 |
{ |
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_stopmotors = 200; // do not repeat if once executed |
delay_stopmotors = 0; // do not repeat if once executed |
Model_Is_Flying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
1231,12 → 1245,28 |
DebugOut.Analog[4] = Reading_GyroYaw; |
DebugOut.Analog[5] = ReadingHeight; |
DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
DebugOut.Analog[8] = CompassHeading; |
// DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[8] = RC_Quality; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
|
#ifdef HEXAKOPTER |
DebugOut.Analog[10] = Motor_FrontLeft; |
DebugOut.Analog[11] = Motor_FrontRight; |
DebugOut.Analog[12] = Motor_RearLeft; |
DebugOut.Analog[13] = Motor_RearRight; |
DebugOut.Analog[14] = Motor_Left; |
DebugOut.Analog[15] = Motor_Right; |
#else |
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
//DebugOut.Analog[16] = Mean_AccTop; |
|
DebugOut.Analog[12] = Motor_Front; |
DebugOut.Analog[13] = Motor_Rear; |
DebugOut.Analog[14] = Motor_Left; |
DebugOut.Analog[15] = Motor_Right; |
#endif |
|
DebugOut.Analog[16] = Mean_AccTop; |
|
DebugOut.Analog[20] = ServoValue; |
|
|
1388,21 → 1418,8 |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
// Motor Front |
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Front = MotorValue; |
NickMixFraction = pd_result; |
|
// Motor Rear |
MotorValue = GasMixFraction - pd_result + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Rear = MotorValue; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1416,20 → 1433,102 |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
RollMixFraction = pd_result; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate Motor Mixes |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
#ifdef HEXAKOPTER |
// Motor FrontLeft |
MotorValue = GasMixFraction |
+ NickMixFraction |
+ RollMixFraction/2 |
- YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_FrontLeft = MotorValue; |
|
// Motor FrontRight |
MotorValue = GasMixFraction |
+ NickMixFraction |
- RollMixFraction/2 |
+ YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_FrontRight = MotorValue; |
|
// Motor RearLeft |
MotorValue = GasMixFraction |
- NickMixFraction |
+ RollMixFraction/2 |
- YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_RearLeft = MotorValue; |
|
// Motor RearRight |
MotorValue = GasMixFraction |
- NickMixFraction |
- RollMixFraction/2 |
+ YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_RearRight= MotorValue; |
|
// Motor Left |
MotorValue = GasMixFraction + pd_result - YawMixFraction; // Mixer |
MotorValue = GasMixFraction |
+ RollMixFraction |
+ YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Left = MotorValue; |
|
// Motor Right |
MotorValue = GasMixFraction - pd_result - YawMixFraction; // Mixer |
// Motor Right |
MotorValue = GasMixFraction |
- RollMixFraction |
- YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Right = MotorValue; |
|
#else |
|
// Motor Front |
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Front = MotorValue; |
|
// Motor Rear |
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Rear = MotorValue; |
|
|
// Motor Left |
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Left = MotorValue; |
|
|
// Motor Right |
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction; // Mixer |
MotorValue /= STICK_GAIN; |
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max; |
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min; |
Motor_Right = MotorValue; |
#endif |
} |
|