rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

uncrustify --- somone forgot to run uncrustify when merging to next -

Browse Source 
again!!! ;)
This commit is contained in:
Corvus Corax 2014-01-11 10:55:50 +01:00
parent 6d3dca6ee3
commit f2bc6508c4
2 changed files with 77 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
flight/modules/ManualControl/manualcontrol.c
View File

@ -691,26 +691,26 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
} }
stabilization.Roll = stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode 0; // this is an invalid mode
stabilization.Pitch = stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode 0; // this is an invalid mode
@ -722,18 +722,17 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
} } else {
else {
stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw = stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
0; // this is an invalid mode 0; // this is an invalid mode
} }

107
flight/modules/Stabilization/stabilization.c
View File

@ -252,9 +252,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#else /* if defined(PIOS_QUATERNION_STABILIZATION) */ #else /* if defined(PIOS_QUATERNION_STABILIZATION) */
// Simpler algorithm for CC, less memory // Simpler algorithm for CC, less memory
float local_error[3] = { stabDesired.Roll - attitudeState.Roll, float local_error[3] = { stabDesired.Roll - attitudeState.Roll,
stabDesired.Pitch - attitudeState.Pitch, stabDesired.Pitch - attitudeState.Pitch,
stabDesired.Yaw - attitudeState.Yaw }; stabDesired.Yaw - attitudeState.Yaw };
// find shortest way // find shortest way
float modulo = fmodf(local_error[2] + 180.0f, 360.0f); float modulo = fmodf(local_error[2] + 180.0f, 360.0f);
if (modulo < 0) { if (modulo < 0) {
@ -320,15 +320,15 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
// A parameterization from Attitude mode at center stick // A parameterization from Attitude mode at center stick
// - to Rate mode at full stick // - to Rate mode at full stick
// This is done by parameterizing to use the rotation rate that Attitude mode // This is done by parameterizing to use the rotation rate that Attitude mode
// - would use at center stick to using the rotation rate that Rate mode // - would use at center stick to using the rotation rate that Rate mode
// - would use at full stick in a weighted average sort of way. // - would use at full stick in a weighted average sort of way.
{ {
if (reinit) { if (reinit) {
pids[PID_ROLL + i].iAccumulator = 0; pids[PID_ROLL + i].iAccumulator = 0;
pids[PID_RATE_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0;
pids[PID_RATEA_ROLL + i].iAccumulator = 0; pids[PID_RATEA_ROLL + i].iAccumulator = 0;
} }
@ -336,7 +336,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Save Rate's rate in a temp for later merging with Attitude's rate // Save Rate's rate in a temp for later merging with Attitude's rate
float rateDesiredAxisRate; float rateDesiredAxisRate;
rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f) rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
* cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]; * cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate // Compute what Attitude mode would give for this stick angle's rate
@ -345,8 +345,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// - subtract off the actual angle to get the angle error // - subtract off the actual angle to get the angle error
// This is what local_error[] holds for Attitude mode // This is what local_error[] holds for Attitude mode
float attitude_error = stabDesiredAxis[i] float attitude_error = stabDesiredAxis[i]
* cast_struct_to_array(settings.RollMax, settings.RollMax)[i] * cast_struct_to_array(settings.RollMax, settings.RollMax)[i]
- cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i]; - cast_struct_to_array(attitudeState.Roll, attitudeState.Roll)[i];
// Compute the outer loop just like Attitude mode does // Compute the outer loop just like Attitude mode does
float rateDesiredAxisAttitude; float rateDesiredAxisAttitude;
@ -363,14 +363,14 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch); // magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude; float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch)); magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate; + magnitude * rateDesiredAxisRate;
// Compute the inner loop for both Rate mode and Attitude mode // Compute the inner loop for both Rate mode and Attitude mode
// actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates // actuatorDesiredAxis[i] is the weighted average of the two PIDs from the two rates
actuatorDesiredAxis[i] = actuatorDesiredAxis[i] =
(1.0f-magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT) (1.0f - magnitude) * pid_apply_setpoint(&pids[PID_RATEA_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT)
+ magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); + magnitude * pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
// settings.RattitudeAntiWindup controls the iAccumulator zeroing // settings.RattitudeAntiWindup controls the iAccumulator zeroing
@ -408,28 +408,28 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// the 7.966 and 17.668 cancel the default PID value and dT given to log2f() // the 7.966 and 17.668 cancel the default PID value and dT given to log2f()
// if these are non-default, tweaking is thus done so the user doesn't have to readjust // if these are non-default, tweaking is thus done so the user doesn't have to readjust
// the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22 // the default value of 10 for UAVO RattitudeAntiWindup gives a power of 22
// these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits // these calculations are for magnitude = 0.5, so 22 corresponds to the number of bits
// used in the mantissa of the float // used in the mantissa of the float
// i.e. 1.0-(0.5^22) almost underflows // i.e. 1.0-(0.5^22) almost underflows
// This may only be useful for aircraft with large Ki values and limits // This may only be useful for aircraft with large Ki values and limits
if (dT > 0.0f && rattitude_anti_windup > 0.0f) { if (dT > 0.0f && rattitude_anti_windup > 0.0f) {
float factor; float factor;
// At magnitudes close to one, the Attitude accumulators gets zeroed // At magnitudes close to one, the Attitude accumulators gets zeroed
if (pids[PID_ROLL+i].i > 0.0f) { if (pids[PID_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t) (32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL+i].i))) - rattitude_anti_windup); factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 7.966f - stab_log2f(dT * pids[PID_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_ROLL+i].iAccumulator *= factor; pids[PID_ROLL + i].iAccumulator *= factor;
} }
if (pids[PID_RATEA_ROLL+i].i > 0.0f) { if (pids[PID_RATEA_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(magnitude, ((uint8_t) (32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL+i].i))) - rattitude_anti_windup); factor = 1.0f - stab_powf(magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATEA_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATEA_ROLL+i].iAccumulator *= factor; pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
} }
// At magnitudes close to zero, the Rate accumulator gets zeroed // At magnitudes close to zero, the Rate accumulator gets zeroed
if (pids[PID_RATE_ROLL+i].i > 0.0f) { if (pids[PID_RATE_ROLL + i].i > 0.0f) {
factor = 1.0f - stab_powf(1.0f-magnitude, ((uint8_t) (32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL+i].i))) - rattitude_anti_windup); factor = 1.0f - stab_powf(1.0f - magnitude, ((uint8_t)(32.1f - 17.668f - stab_log2f(dT * pids[PID_RATE_ROLL + i].i))) - rattitude_anti_windup);
pids[PID_RATE_ROLL+i].iAccumulator *= factor; pids[PID_RATE_ROLL + i].iAccumulator *= factor;
} }
} }
@ -447,12 +447,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling // FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are: // The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp // Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp // That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account // Also does not take dT into account
{ {
if (reinit) { if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0;
@ -617,38 +617,35 @@ static float bound(float val, float range)
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable // x small (0.0 < x < .01) so interpolation of fractional part is reasonable
static float stab_log2f(float x) static float stab_log2f(float x)
{ {
union union {
{ volatile float f;
volatile float f; volatile uint32_t i;
volatile uint32_t i; volatile unsigned char c[4];
volatile unsigned char c[4]; } __attribute__((packed)) u1, u2;
} __attribute__((packed)) u1, u2;
u2.f = u1.f = x; u2.f = u1.f = x;
u1.i <<= 1; u1.i <<= 1;
u2.i &= 0xff800000; u2.i &= 0xff800000;
// get and unbias the exponent, add in a linear interpolation of the fractional part // get and unbias the exponent, add in a linear interpolation of the fractional part
return (float) (u1.c[3] - 127) + (x / u2.f) - 1.0f; return (float)(u1.c[3] - 127) + (x / u2.f) - 1.0f;
} }
// 0<=x<=1, 0<=p<=31 // 0<=x<=1, 0<=p<=31
static float stab_powf(float x, uint8_t p) static float stab_powf(float x, uint8_t p)
{ {
float retval = 1.0f; float retval = 1.0f;
while (p) while (p) {
{ if (p & 1) {
if (p&1) retval *= x;
{ }
retval *= x; x *= x;
p >>= 1;
} }
x *= x;
p >>= 1;
}
return retval; return retval;
} }