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

OP-984 More changes to stabilization.c for bank switching

Browse Source
This commit is contained in:
Les Newell 2013-12-10 20:10:01 +00:00
parent 114c0ac561
commit fddf61377e
1 changed files with 29 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -91,6 +91,7 @@ bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES]; bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f; float vbar_decay = 0.991f;
struct pid pids[PID_MAX]; struct pid pids[PID_MAX];
int flight_mode = -1;
// Private functions // Private functions
static void stabilizationTask(void *parameters); static void stabilizationTask(void *parameters);
@ -201,6 +202,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#ifdef DIAG_RATEDESIRED #ifdef DIAG_RATEDESIRED
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
#endif #endif
if(flight_mode != flightStatus.FlightMode){
BankChanged();
flight_mode = flightStatus.FlightMode;
}
#ifdef REVOLUTION #ifdef REVOLUTION
float speedScaleFactor; float speedScaleFactor;
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes // Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
@ -496,11 +503,12 @@ static float bound(float val, float range)
static void BankChanged() static void BankChanged()
{ {
StabilizationBankData bank; StabilizationBankData bank, oldBank;
FlightStatusData flightStatus; FlightStatusData flightStatus;
StabilizationBankGet(&bank); StabilizationBankGet(&bank);
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
memcpy(&oldBank, &bank, sizeof(StabilizationBankData));
int offset = flightStatus.FlightMode; int offset = flightStatus.FlightMode;
bank.RollMax = settings.RollMax[offset]; bank.RollMax = settings.RollMax[offset];
@ -521,7 +529,11 @@ static void BankChanged()
memcpy(&bank.PitchPI, (&settings.PitchPI) + offset, sizeof(float) * ATT_OFFSET); memcpy(&bank.PitchPI, (&settings.PitchPI) + offset, sizeof(float) * ATT_OFFSET);
memcpy(&bank.YawPI, (&settings.YawPI) + offset, sizeof(float) * ATT_OFFSET); memcpy(&bank.YawPI, (&settings.YawPI) + offset, sizeof(float) * ATT_OFFSET);
StabilizationBankSet(&bank); //Need to do this to prevent an infinite loop
if(memcmp(&oldBank, &bank, sizeof(StabilizationBankData)) != 0)
{
StabilizationBankSet(&bank);
}
} }
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
@ -532,73 +544,31 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
StabilizationBankGet(&bank); StabilizationBankGet(&bank);
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
bool change = false; StabilizationSettingsData oldSettings;
memcpy(&oldSettings, &settings, sizeof(StabilizationSettingsData));
int offset = flightStatus.FlightMode; int offset = flightStatus.FlightMode;
if(bank.RollMax != settings.RollMax[offset]) settings.RollMax[offset] = bank.RollMax;
{ settings.PitchMax[offset] = bank.PitchMax;
change = true; settings.YawMax[offset] = bank.YawMax;
settings.RollMax[offset] = bank.RollMax;
}
if(bank.PitchMax != settings.PitchMax[offset])
{
change = true;
settings.PitchMax[offset] = bank.PitchMax;
}
if(bank.YawMax != settings.YawMax[offset])
{
change = true;
settings.YawMax[offset] = bank.YawMax;
}
offset = flightStatus.FlightMode * MAX_AXES; offset = flightStatus.FlightMode * MAX_AXES;
if(memcmp(&bank.ManualRate, (&settings.ManualRate) + offset, sizeof(float) * MAX_AXES) != 0) memcpy((&settings.ManualRate) + offset, &bank.ManualRate, sizeof(float) * MAX_AXES);
{ memcpy((&settings.MaximumRate) + offset, &bank.MaximumRate, sizeof(float) * MAX_AXES);
change = true;
memcpy((&settings.ManualRate) + offset, &bank.ManualRate, sizeof(float) * MAX_AXES);
}
if(memcmp(&bank.MaximumRate, (&settings.MaximumRate) + offset, sizeof(float) * MAX_AXES) != 0)
{
change = true;
memcpy((&settings.MaximumRate) + offset, &bank.MaximumRate, sizeof(float) * MAX_AXES);
}
offset = flightStatus.FlightMode * RATE_OFFSET; offset = flightStatus.FlightMode * RATE_OFFSET;
if(memcmp(&bank.RollRatePID, (&settings.RollRatePID) + offset, sizeof(float) * RATE_OFFSET) != 0) memcpy((&settings.RollRatePID) + offset, &bank.RollRatePID, sizeof(float) * RATE_OFFSET);
{ memcpy((&settings.PitchRatePID) + offset, &bank.PitchRatePID, sizeof(float) * RATE_OFFSET);
change = true; memcpy((&settings.YawRatePID) + offset, &bank.YawRatePID, sizeof(float) * RATE_OFFSET);
memcpy((&settings.RollRatePID) + offset, &bank.RollRatePID, sizeof(float) * RATE_OFFSET);
}
if(memcmp(&bank.PitchRatePID, (&settings.PitchRatePID) + offset, sizeof(float) * RATE_OFFSET) != 0)
{
change = true;
memcpy((&settings.PitchRatePID) + offset, &bank.PitchRatePID, sizeof(float) * RATE_OFFSET);
}
if(memcmp(&bank.YawRatePID, (&settings.YawRatePID) + offset, sizeof(float) * RATE_OFFSET) != 0)
{
change = true;
memcpy((&settings.YawRatePID) + offset, &bank.YawRatePID, sizeof(float) * RATE_OFFSET);
}
offset = flightStatus.FlightMode * ATT_OFFSET; offset = flightStatus.FlightMode * ATT_OFFSET;
if(memcmp(&bank.RollPI, (&settings.RollPI) + offset, sizeof(float) * ATT_OFFSET) != 0) memcpy((&settings.RollPI) + offset, &bank.RollPI, sizeof(float) * ATT_OFFSET);
{ memcpy((&settings.PitchPI) + offset, &bank.PitchPI, sizeof(float) * ATT_OFFSET);
change = true; memcpy((&settings.YawPI) + offset, &bank.YawPI, sizeof(float) * ATT_OFFSET);
memcpy((&settings.RollPI) + offset, &bank.RollPI, sizeof(float) * ATT_OFFSET);
}
if(memcmp(&bank.PitchPI, (&settings.PitchPI) + offset, sizeof(float) * ATT_OFFSET) != 0)
{
change = true;
memcpy((&settings.PitchPI) + offset, &bank.PitchPI, sizeof(float) * ATT_OFFSET);
}
if(memcmp(&bank.YawPI, (&settings.YawPI) + offset, sizeof(float) * ATT_OFFSET) != 0)
{
change = true;
memcpy((&settings.YawPI) + offset, &bank.YawPI, sizeof(float) * ATT_OFFSET);
}
if(change) //Need to do this to prevent an infinite loop
if(memcmp(&oldSettings, &settings, sizeof(StabilizationSettingsData)) != 0)
{ {
StabilizationSettingsSet(&settings); StabilizationSettingsSet(&settings);
} }