rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Whitespace fixes

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2931 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-03-02 01:25:30 +00:00 committed by peabody124
parent 3e17c2ff55
commit 3488d28a5b
1 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
flight/Modules/Stabilization/stabilization.c
View File

@ -90,14 +90,14 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
int32_t StabilizationInitialize() int32_t StabilizationInitialize()
{ {
// Initialize variables // Initialize variables
// Create object queue // Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates. // Listen for updates.
// AttitudeActualConnectQueue(queue); // AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue); AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb); StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle()); SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task // Start main task
@ -116,8 +116,8 @@ static void stabilizationTask(void* parameters)
portTickType lastSysTime; portTickType lastSysTime;
portTickType thisSysTime; portTickType thisSysTime;
UAVObjEvent ev; UAVObjEvent ev;
ActuatorDesiredData actuatorDesired; ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
RateDesiredData rateDesired; RateDesiredData rateDesired;
@ -125,15 +125,15 @@ static void stabilizationTask(void* parameters)
AttitudeRawData attitudeRaw; AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings; SystemSettingsData systemSettings;
ManualControlCommandData manualControl; ManualControlCommandData manualControl;
SettingsUpdatedCb((UAVObjEvent *) NULL); SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
ZeroPids(); ZeroPids();
while(1) { while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{ {
@ -146,36 +146,36 @@ static void stabilizationTask(void* parameters)
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
ManualControlCommandGet(&manualControl); ManualControlCommandGet(&manualControl);
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw); AttitudeRawGet(&attitudeRaw);
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings); SystemSettingsGet(&systemSettings);
float *attitudeDesiredAxis = &stabDesired.Roll; float *attitudeDesiredAxis = &stabDesired.Roll;
float *attitudeActualAxis = &attitudeActual.Roll; float *attitudeActualAxis = &attitudeActual.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate //Calculate desired rate
for(int8_t ct=0; ct< MAX_AXES; ct++) for(int8_t ct=0; ct< MAX_AXES; ct++)
{ {
switch(stabDesired.StabilizationMode[ct]) switch(stabDesired.StabilizationMode[ct])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[ct] = attitudeDesiredAxis[ct]; rateDesiredAxis[ct] = attitudeDesiredAxis[ct];
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1); rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1);
break; break;
} }
} }
uint8_t shouldUpdate = 1; uint8_t shouldUpdate = 0;
RateDesiredSet(&rateDesired); RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired); ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command //Calculate desired command
@ -190,12 +190,12 @@ static void stabilizationTask(void* parameters)
{ {
rateDesiredAxis[ct] = -settings.MaximumRate[ct]; rateDesiredAxis[ct] = -settings.MaximumRate[ct];
} }
} }
switch(stabDesired.StabilizationMode[ct]) switch(stabDesired.StabilizationMode[ct])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
{ {
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0); float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct], attitudeRaw.gyros[ct], 0);
actuatorDesiredAxis[ct] = bound(command); actuatorDesiredAxis[ct] = bound(command);
@ -223,16 +223,16 @@ static void stabilizationTask(void* parameters)
} }
} }
// Save dT // Save dT
actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.UpdateTime = dT * 1000;
if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) if(manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL)
{ {
shouldUpdate = 0; shouldUpdate = 0;
} }
if(shouldUpdate) if(shouldUpdate)
{ {
actuatorDesired.Throttle = stabDesired.Throttle; actuatorDesired.Throttle = stabDesired.Throttle;
@ -240,9 +240,9 @@ static void stabilizationTask(void* parameters)
actuatorDesired.NumLongUpdates++; actuatorDesired.NumLongUpdates++;
ActuatorDesiredSet(&actuatorDesired); ActuatorDesiredSet(&actuatorDesired);
} }
if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE || if(manualControl.Armed == MANUALCONTROLCOMMAND_ARMED_FALSE ||
!shouldUpdate || (stabDesired.Throttle < 0)) !shouldUpdate || (stabDesired.Throttle < 0))
{ {
ZeroPids(); ZeroPids();
} }
@ -266,7 +266,7 @@ float ApplyPid(pid_type * pid, const float desired, const float actual, const ui
err += 360; err += 360;
} }
} }
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
pid->lastErr = err; pid->lastErr = err;
pid->iAccumulator += err * pid->i * dT; pid->iAccumulator += err * pid->i * dT;
@ -308,7 +308,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
{ {
memset(pids,0,sizeof (pid_type) * PID_MAX); memset(pids,0,sizeof (pid_type) * PID_MAX);
StabilizationSettingsGet(&settings); StabilizationSettingsGet(&settings);
float * data = settings.RollRatePI; float * data = settings.RollRatePI;
for(int8_t pid=0; pid < PID_MAX; pid++) for(int8_t pid=0; pid < PID_MAX; pid++)
{ {
@ -320,6 +320,6 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
/** /**
* @} * @}
* @} * @}
*/ */