Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 844 → Rev 1087

/branches/V0.68d CRK HexaLotte/fc.c
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;