mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
moved Vassilis stabilization module as of r1153 into Stabilization/simple
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1173 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
corvus
corvus
parent
9ad9ffd466
commit
02a78f0470
@ -0,0 +1,43 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup StabilizationModule Stabilization Module
|
||||
* @brief Stabilization PID loops in an airframe type independent manner
|
||||
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
|
||||
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
|
||||
* @{
|
||||
*
|
||||
* @file stabilization.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef STABILIZATION_H
|
||||
#define STABILIZATION_H
|
||||
|
||||
int32_t StabilizationInitialize();
|
||||
|
||||
#endif // STABILIZATION_H
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -0,0 +1,193 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup StabilizationModule Stabilization Module
|
||||
* @brief Stabilization PID loops in an airframe type independent manner
|
||||
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
|
||||
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeActual "Attitude Actual"
|
||||
* @{
|
||||
*
|
||||
* @file stabilization.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "stabilization.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "attitudedesired.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "systemsettings.h"
|
||||
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define PITCH_INTEGRAL_LIMIT 0.5
|
||||
#define ROLL_INTEGRAL_LIMIT 0.5
|
||||
#define YAW_INTEGRAL_LIMIT 0.5
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationTask(void* parameters);
|
||||
static float bound(float val, float min, float max);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t StabilizationInitialize()
|
||||
{
|
||||
// Initialize variables
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module task
|
||||
*/
|
||||
static void stabilizationTask(void* parameters)
|
||||
{
|
||||
StabilizationSettingsData stabSettings;
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
AttitudeDesiredData attitudeDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
ManualControlCommandData manualControl;
|
||||
SystemSettingsData systemSettings;
|
||||
portTickType lastSysTime;
|
||||
float pitchError, pitchErrorLast;
|
||||
float rollError, rollErrorLast;
|
||||
float yawError, yawErrorLast;
|
||||
float pitchDerivative;
|
||||
float rollDerivative;
|
||||
float yawDerivative;
|
||||
float pitchIntegral, pitchIntegralLimit;
|
||||
float rollIntegral, rollIntegralLimit;
|
||||
float yawIntegral, yawIntegralLimit;
|
||||
|
||||
// Initialize
|
||||
pitchIntegral = 0.0;
|
||||
rollIntegral = 0.0;
|
||||
yawIntegral = 0.0;
|
||||
pitchErrorLast = 0.0;
|
||||
rollErrorLast = 0.0;
|
||||
yawErrorLast = 0.0;
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
// Read settings and other objects
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
ManualControlCommandGet(&manualControl);
|
||||
AttitudeDesiredGet(&attitudeDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
|
||||
// Pitch stabilization control loop
|
||||
pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch;
|
||||
pitchDerivative = pitchError - pitchErrorLast;
|
||||
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
|
||||
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
|
||||
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
|
||||
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
|
||||
pitchErrorLast = pitchError;
|
||||
|
||||
// Roll stabilization control loop
|
||||
rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll;
|
||||
rollDerivative = rollError - rollErrorLast;
|
||||
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
|
||||
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
|
||||
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
|
||||
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
|
||||
rollErrorLast = rollError;
|
||||
|
||||
// Yaw stabilization control loop (only enabled on VTOL airframes)
|
||||
if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )
|
||||
{
|
||||
yawError = attitudeDesired.Yaw - attitudeActual.Yaw;
|
||||
yawDerivative = yawError - yawErrorLast;
|
||||
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
|
||||
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
|
||||
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
|
||||
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
|
||||
yawErrorLast = yawError;
|
||||
}
|
||||
else
|
||||
{
|
||||
actuatorDesired.Yaw = 0.0;
|
||||
}
|
||||
|
||||
// Setup throttle
|
||||
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
|
||||
|
||||
// Write actuator desired (if not in manual mode)
|
||||
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
|
||||
{
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
}
|
||||
else
|
||||
{
|
||||
pitchIntegral = 0.0;
|
||||
rollIntegral = 0.0;
|
||||
yawIntegral = 0.0;
|
||||
pitchErrorLast = 0.0;
|
||||
rollErrorLast = 0.0;
|
||||
yawErrorLast = 0.0;
|
||||
}
|
||||
|
||||
// Clear alarms
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
|
||||
// Wait until next update
|
||||
vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Bound input value between limits
|
||||
*/
|
||||
static float bound(float val, float min, float max)
|
||||
{
|
||||
if ( val < min )
|
||||
{
|
||||
val = min;
|
||||
}
|
||||
else if ( val > max )
|
||||
{
|
||||
val = max;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
Loading…
Reference in New Issue
Block a user