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

PiOS.win32: Updated PiOS.win32 makefile for recent UAVObject changes, and also updated the experimental stabilization code to comply with recent changes to the simple code. Nobody uses SiTL anymore :( .

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1858 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
cwabbott 2010-10-03 18:59:58 +00:00 committed by cwabbott
parent 11cf24a312
commit 8d1a99ae15
2 changed files with 66 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
flight/OpenPilot/Makefile.win32
View File

@ -53,7 +53,7 @@ FLASH_TOOL = OPENOCD
USE_THUMB_MODE = YES
# List of modules to include
MODULES = Telemetry Stabilization/experimental/Stabilization Navigation ManualControl
MODULES = Telemetry Stabilization/experimental/Stabilization ManualControl
#MODULES = Telemetry GPS ManualControl Actuator Altitude Attitude Stabilization
#MODULES = Telemetry Example
#MODULES = Telemetry MK/MKSerial
@ -124,7 +124,6 @@ SRC += $(OPTESTS)/$(TESTAPP).c
endif
## UAVOBJECTS
ifndef TESTAPP
SRC += $(OPUAVOBJ)/exampleobject1.c
@ -133,6 +132,8 @@ SRC += $(OPUAVOBJ)/examplesettings.c
SRC += $(OPUAVOBJ)/objectpersistence.c
SRC += $(OPUAVOBJ)/positionactual.c
SRC += $(OPUAVOBJ)/gpsposition.c
SRC += $(OPUAVOBJ)/gpstime.c
SRC += $(OPUAVOBJ)/gpssatellites.c
SRC += $(OPUAVOBJ)/gcstelemetrystats.c
SRC += $(OPUAVOBJ)/flighttelemetrystats.c
SRC += $(OPUAVOBJ)/systemstats.c
@ -148,18 +149,21 @@ SRC += $(OPUAVOBJ)/attitudedesired.c
SRC += $(OPUAVOBJ)/stabilizationsettings.c
SRC += $(OPUAVOBJ)/ahrsstatus.c
SRC += $(OPUAVOBJ)/baroaltitude.c
SRC += $(OPUAVOBJ)/ahrscalibration.c
SRC += $(OPUAVOBJ)/attitudeactual.c
SRC += $(OPUAVOBJ)/flightsituationactual.c
SRC += $(OPUAVOBJ)/navigationsettings.c
SRC += $(OPUAVOBJ)/navigationdesired.c
SRC += $(OPUAVOBJ)/ahrssettings.c
SRC += $(OPUAVOBJ)/flightbatterystate.c
SRC += $(OPUAVOBJ)/attituderaw.c
SRC += $(OPUAVOBJ)/homelocation.c
SRC += $(OPUAVOBJ)/ahrscalibration.c
SRC += $(OPUAVOBJ)/ahrssettings.c
SRC += $(OPUAVOBJ)/gpstime.c
SRC += $(OPUAVOBJ)/gpssatellites.c
SRC += $(OPUAVOBJ)/attitudesettings.c
SRC += $(OPUAVOBJ)/vtolsettings.c
SRC += $(OPUAVOBJ)/mixersettings.c
SRC += $(OPUAVOBJ)/mixerstatus.c
#SRC += $(OPUAVOBJ)/lesstabilizationsettings.c
SRC += $(OPUAVOBJ)/positiondesired.c
SRC += $(OPUAVOBJ)/velocitydesired.c
SRC += $(OPUAVOBJ)/velocityactual.c
SRC += $(OPUAVOBJ)/guidancesettings.c
endif
## PIOS Hardware (win32)

63
flight/OpenPilot/Modules/Stabilization/experimental/Stabilization/stabilization.c
View File

@ -42,16 +42,20 @@
// Private constants
#define MAX_QUEUE_SIZE 2
#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
#define DEG2RAD ( M_PI / 180.0 )
#define FAILSAFE_TIMEOUT_MS 100
// Private types
// Private variables
static xQueueHandle queue;
static xTaskHandle taskHandle;
// Private functions
@ -64,19 +68,26 @@ static float angleDifference(float val);
*/
int32_t StabilizationInitialize()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for AttitudeActual updates.
AttitudeActualConnectQueue(queue);
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
return 0;
}
float dT = 1;
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
@ -84,6 +95,7 @@ static void stabilizationTask(void* parameters)
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
portTickType thisSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
@ -95,6 +107,8 @@ static void stabilizationTask(void* parameters)
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
float yawPrevious;
float yawChange;
// Initialize
pitchIntegral = 0.0;
@ -103,11 +117,24 @@ static void stabilizationTask(void* parameters)
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
yawPrevious = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
@ -132,10 +159,21 @@ static void stabilizationTask(void* parameters)
}
// global yaw error
if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL || manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
yawErrorGlobal = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
yawPrevious = attitudeActual.Yaw;
yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
} else { // heading stabilization
yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
}
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
@ -174,25 +212,30 @@ static void stabilizationTask(void* parameters)
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
@ -205,6 +248,9 @@ static void stabilizationTask(void* parameters)
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
@ -222,9 +268,6 @@ static void stabilizationTask(void* parameters)
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Wait until next update
vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
}
}