rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

OP-984 Changed stabilization.c to handle multiple pid bank objects

Browse Source
This commit is contained in:
Les Newell 2013-12-11 13:56:00 +00:00
parent 641cf474eb
commit 68b8586c6b
1 changed files with 70 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,6 +36,9 @@
#include "stabilization.h" #include "stabilization.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "stabilizationbank.h" #include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "ratedesired.h" #include "ratedesired.h"
#include "relaytuning.h" #include "relaytuning.h"
@ -97,9 +100,9 @@ int flight_mode = -1;
static void stabilizationTask(void *parameters); static void stabilizationTask(void *parameters);
static float bound(float val, float range); static float bound(float val, float range);
static void ZeroPids(void); static void ZeroPids(void);
static void BankChanged();
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev); static void BankUpdatedCb(UAVObjEvent *ev);
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
/** /**
* Module initialization * Module initialization
@ -119,6 +122,11 @@ int32_t StabilizationStart()
StabilizationBankConnectCallback(BankUpdatedCb); StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
// Start main task // Start main task
xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(stabilizationTask, (signed char *)"Stabilization", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -136,6 +144,9 @@ int32_t StabilizationInitialize()
// Initialize variables // Initialize variables
StabilizationSettingsInitialize(); StabilizationSettingsInitialize();
StabilizationBankInitialize(); StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
ActuatorDesiredInitialize(); ActuatorDesiredInitialize();
#ifdef DIAG_RATEDESIRED #ifdef DIAG_RATEDESIRED
RateDesiredInitialize(); RateDesiredInitialize();
@ -205,8 +216,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
#endif #endif
if(flight_mode != flightStatus.FlightMode){ if(flight_mode != flightStatus.FlightMode){
BankChanged();
flight_mode = flightStatus.FlightMode; flight_mode = flightStatus.FlightMode;
SettingsBankUpdatedCb(NULL);
} }
#ifdef REVOLUTION #ifdef REVOLUTION
@ -502,36 +513,35 @@ static float bound(float val, float range)
return val; return val;
} }
static void BankChanged()
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
StabilizationBankData bank, oldBank; StabilizationBankData bank, oldBank;
FlightStatusData flightStatus; StabilizationBankGet(&oldBank);
StabilizationBankGet(&bank); if(flight_mode < 0) return;
FlightStatusGet(&flightStatus);
memcpy(&oldBank, &bank, sizeof(StabilizationBankData));
int offset = flightStatus.FlightMode; switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
bank.RollMax = settings.RollMax[offset]; {
bank.PitchMax = settings.PitchMax[offset]; case 0:
bank.YawMax = settings.YawMax[offset]; StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &bank);
break;
offset = flightStatus.FlightMode * MAX_AXES; case 1:
memcpy(&bank.ManualRate, (&settings.ManualRate) + offset, sizeof(float) * MAX_AXES); StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &bank);
memcpy(&bank.MaximumRate, (&settings.MaximumRate) + offset, sizeof(float) * MAX_AXES); break;
offset = flightStatus.FlightMode * RATE_OFFSET; case 2:
memcpy(&bank.RollRatePID, (&settings.RollRatePID) + offset, sizeof(float) * RATE_OFFSET); StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &bank);
memcpy(&bank.PitchRatePID, (&settings.PitchRatePID) + offset, sizeof(float) * RATE_OFFSET); break;
memcpy(&bank.YawRatePID, (&settings.YawRatePID) + offset, sizeof(float) * RATE_OFFSET);
offset = flightStatus.FlightMode * ATT_OFFSET; default:
memcpy(&bank.RollPI, (&settings.RollPI) + offset, sizeof(float) * ATT_OFFSET); return; //bank number is invalid. All we can do is ignore it.
memcpy(&bank.PitchPI, (&settings.PitchPI) + offset, sizeof(float) * ATT_OFFSET); }
memcpy(&bank.YawPI, (&settings.YawPI) + offset, sizeof(float) * ATT_OFFSET);
//Need to do this to prevent an infinite loop //Need to do this to prevent an infinite loop
if(memcmp(&oldBank, &bank, sizeof(StabilizationBankData)) != 0) if(memcmp(&oldBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{ {
StabilizationBankSet(&bank); StabilizationBankSet(&bank);
} }
@ -539,49 +549,40 @@ static void BankChanged()
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
StabilizationBankData bank; StabilizationBankData curBank, bank;
FlightStatusData flightStatus;
StabilizationBankGet(&bank); StabilizationBankGet(&bank);
FlightStatusGet(&flightStatus);
StabilizationSettingsData oldSettings; if(flight_mode < 0) return;
memcpy(&oldSettings, &settings, sizeof(StabilizationSettingsData));
int offset = flightStatus.FlightMode; switch(cast_struct_to_array(settings.FlightModeMap, settings.FlightModeMap.Stabilized1)[flight_mode])
settings.RollMax[offset] = bank.RollMax;
settings.PitchMax[offset] = bank.PitchMax;
settings.YawMax[offset] = bank.YawMax;
offset = flightStatus.FlightMode * MAX_AXES;
memcpy((&settings.ManualRate) + offset, &bank.ManualRate, sizeof(float) * MAX_AXES);
memcpy((&settings.MaximumRate) + offset, &bank.MaximumRate, sizeof(float) * MAX_AXES);
offset = flightStatus.FlightMode * RATE_OFFSET;
memcpy((&settings.RollRatePID) + offset, &bank.RollRatePID, sizeof(float) * RATE_OFFSET);
memcpy((&settings.PitchRatePID) + offset, &bank.PitchRatePID, sizeof(float) * RATE_OFFSET);
memcpy((&settings.YawRatePID) + offset, &bank.YawRatePID, sizeof(float) * RATE_OFFSET);
offset = flightStatus.FlightMode * ATT_OFFSET;
memcpy((&settings.RollPI) + offset, &bank.RollPI, sizeof(float) * ATT_OFFSET);
memcpy((&settings.PitchPI) + offset, &bank.PitchPI, sizeof(float) * ATT_OFFSET);
memcpy((&settings.YawPI) + offset, &bank.YawPI, sizeof(float) * ATT_OFFSET);
//Need to do this to prevent an infinite loop
if(memcmp(&oldSettings, &settings, sizeof(StabilizationSettingsData)) != 0)
{ {
StabilizationSettingsSet(&settings); case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *) &bank);
}
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *) &bank);
}
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *) &curBank);
if(memcmp(&curBank, &bank, sizeof(StabilizationBankDataPacked)) != 0)
{
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *) &bank);
}
break;
default:
return; //invalid bank number
} }
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
BankChanged();
StabilizationBankData bank;
StabilizationBankGet(&bank);
// Set the roll rate PID constants // Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp, pid_configure(&pids[PID_RATE_ROLL], bank.RollRatePID.Kp,
@ -618,6 +619,12 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
bank.YawPI.Ki, bank.YawPI.Ki,
0, 0,
bank.YawPI.ILimit); bank.YawPI.ILimit);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationSettingsGet(&settings);
// Set up the derivative term // Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -652,6 +659,8 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// Compute time constant for vbar decay term based on a tau // Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau); vbar_decay = expf(-fakeDt / settings.VbarTau);
flight_mode = -1; //force flight mode update
} }