118,7 → 118,7 |
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; |
volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left; |
|
// stick values derived by rc channels readings |
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0; |
359,21 → 359,25 |
{ |
if(MOTOR_OFF || !MotorsOn) |
{ |
Motor_Rear = 0; |
Motor_Front = 0; |
Motor_RearLeft = 0; |
Motor_RearRight = 0; |
Motor_FrontLeft = 0; |
Motor_FrontRight = 0; |
Motor_Right = 0; |
Motor_Left = 0; |
if(MotorTest[0]) Motor_Front = MotorTest[0]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
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]; |
} |
|
//DebugOut.Analog[12] = Motor_Front; |
//DebugOut.Analog[13] = Motor_Rear; |
//DebugOut.Analog[14] = Motor_Left; |
//DebugOut.Analog[15] = Motor_Right; |
|
DebugOut.Analog[12] = Motor_FrontLeft; |
DebugOut.Analog[13] = Motor_RearRight; |
DebugOut.Analog[14] = Motor_FrontRight; |
DebugOut.Analog[15] = Motor_RearLeft; |
DebugOut.Analog[16] = Motor_Left; |
DebugOut.Analog[17] = Motor_Right; |
|
//Start I2C Interrupt Mode |
twi_state = 0; |
motor = 0; |
424,7 → 428,7 |
void MotorControl(void) |
{ |
int16_t MotorValue, pd_result, h, tmp_int; |
int16_t YawMixFraction, ThrustMixFraction; |
int16_t YawMixFraction, ThrustMixFraction, PitchMixFraction, RollMixFraction; |
static int32_t SumPitch = 0, SumRoll = 0; |
static int32_t SetPointYaw = 0; |
static int32_t IntegralErrorPitch = 0; |
887,7 → 891,7 |
Reading_IntegralGyroRoll2 -= IntegralErrorRoll; |
|
|
DebugOut.Analog[17] = IntegralAccPitch / 26; |
// DebugOut.Analog[17] = IntegralAccPitch / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
DebugOut.Analog[19] = IntegralErrorPitch;// / 26; |
DebugOut.Analog[20] = IntegralErrorRoll;// / 26; |
1091,7 → 1095,7 |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
//DebugOut.Analog[11] = RC_Quality; |
DebugOut.Analog[16] = Mean_AccTop; |
//DebugOut.Analog[16] = Mean_AccTop; |
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
1199,6 → 1203,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mixer and PI-Controller |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
DebugOut.Analog[7] = ThrustMixFraction; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Yaw-Fraction |
1211,6 → 1216,7 |
if(YawMixFraction > ((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = ((ParamSet.Trust_Max - ThrustMixFraction)); |
if(YawMixFraction < -((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = -((ParamSet.Trust_Max - ThrustMixFraction)); |
if(ThrustMixFraction < 20) YawMixFraction = 0; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Pitch-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1224,20 → 1230,9 |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
// Motor Front |
MotorValue = ThrustMixFraction + pd_result + YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Front = MotorValue; |
|
// Motor Rear |
MotorValue = ThrustMixFraction - pd_result + YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Rear = MotorValue; |
|
PitchMixFraction = pd_result; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Roll-Axis |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1251,15 → 1246,62 |
if(pd_result > tmp_int) pd_result = tmp_int; |
if(pd_result < -tmp_int) pd_result = -tmp_int; |
|
RollMixFraction = pd_result; |
|
// Calculate Motor Mixes |
|
// Motor FrontLeft |
MotorValue = ThrustMixFraction |
+ PitchMixFraction |
+ RollMixFraction/2 |
- YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_FrontLeft = MotorValue; |
|
// Motor FrontRight |
MotorValue = ThrustMixFraction |
+ PitchMixFraction |
- RollMixFraction/2 |
+ YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_FrontRight = MotorValue; |
|
// Motor RearLeft |
MotorValue = ThrustMixFraction |
- PitchMixFraction |
+ RollMixFraction/2 |
- YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_RearLeft = MotorValue; |
// Motor RearRight |
MotorValue = ThrustMixFraction |
- PitchMixFraction |
- RollMixFraction/2 |
+ YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_RearRight= MotorValue; |
|
// Motor Left |
MotorValue = ThrustMixFraction + pd_result - YawMixFraction; // Mixer |
MotorValue = ThrustMixFraction |
+ RollMixFraction |
+ YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Motor_Left = MotorValue; |
|
// Motor Right |
MotorValue = ThrustMixFraction - pd_result - YawMixFraction; // Mixer |
// Motor Right |
MotorValue = ThrustMixFraction |
- RollMixFraction |
- YawMixFraction; // Mixer |
if ((MotorValue < 0)) MotorValue = 0; |
else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |