mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +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