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

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

@ -722,8 +722,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
}
else {
} else {
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :

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

@ -363,13 +363,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// magnitude = sqrt(stabDesired.Roll*stabDesired.Roll + stabDesired.Pitch*stabDesired.Pitch);
float magnitude;
magnitude = fmaxf(fabsf(stabDesired.Roll), fabsf(stabDesired.Pitch));
rateDesiredAxis[i] = (1.0f-magnitude) * rateDesiredAxisAttitude
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
+ magnitude * rateDesiredAxisRate;
// 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] =
(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);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -417,19 +417,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
float factor;
// At magnitudes close to one, the Attitude accumulators gets zeroed
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);
pids[PID_ROLL+i].iAccumulator *= factor;
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);
pids[PID_ROLL + i].iAccumulator *= factor;
}
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);
pids[PID_RATEA_ROLL+i].iAccumulator *= factor;
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);
pids[PID_RATEA_ROLL + i].iAccumulator *= factor;
}
// At magnitudes close to zero, the Rate accumulator gets zeroed
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);
pids[PID_RATE_ROLL+i].iAccumulator *= factor;
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);
pids[PID_RATE_ROLL + i].iAccumulator *= factor;
}
}
@ -617,8 +617,7 @@ static float bound(float val, float range)
// x small (0.0 < x < .01) so interpolation of fractional part is reasonable
static float stab_log2f(float x)
{
union
{
union {
volatile float f;
volatile uint32_t i;
volatile unsigned char c[4];
@ -629,7 +628,7 @@ static float stab_log2f(float x)
u2.i &= 0xff800000;
// 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;
}
@ -638,10 +637,8 @@ static float stab_powf(float x, uint8_t p)
{
float retval = 1.0f;
while (p)
{
if (p&1)
{
while (p) {
if (p & 1) {
retval *= x;
}
x *= x;