rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

Added basic code for Yaw stabilisation in manual mode for Helis.

Browse Source 
This is functionally the same as having a heading hold gyro in hardware.
Code is copied from the simple stabilisation routine and is a bit rough at the minute, but initial testing looks good.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1670 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
andrew 2010-09-17 05:52:34 +00:00 committed by andrew
parent 41b7a99883
commit da1a762072
1 changed files with 58 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
flight/OpenPilot/Modules/Actuator/actuator.c
View File

@ -40,6 +40,12 @@
#include "actuatorcommand.h" #include "actuatorcommand.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
//yaw stabilisation for helis...
#include "stabilizationsettings.h"
#include "attitudeactual.h"
//yaw stabilisation for helis...
// Private constants // Private constants
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
#define STACK_SIZE configMINIMAL_STACK_SIZE #define STACK_SIZE configMINIMAL_STACK_SIZE
@ -47,6 +53,18 @@
#define FAILSAFE_TIMEOUT_MS 100 #define FAILSAFE_TIMEOUT_MS 100
// Private types // Private types
//yaw stabilisation for helis...
//currently just coppied from simple stabilisation
#define YAW_INTEGRAL_LIMIT 0.5
AttitudeActualData attitudeActual;
StabilizationSettingsData stabSettings;
float ccpmYawError, ccpmYawErrorLast;
float ccpmYawDerivative;
float ccpmYawIntegral, ccpmYawIntegralLimit;
float desiredYaw;
static float bound(float val, float min, float max);
//yaw stabilisation for helis...
// Private variables // Private variables
static xQueueHandle queue; static xQueueHandle queue;
@ -76,6 +94,10 @@ int32_t ActuatorInitialize()
// Listen for ExampleObject1 updates // Listen for ExampleObject1 updates
ActuatorDesiredConnectQueue(queue); ActuatorDesiredConnectQueue(queue);
ccpmYawErrorLast = 0.0;
// Start main task // Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
@ -563,10 +585,46 @@ static int32_t mixerCCPM( ActuatorSettingsData* settings, const ActuatorDesiredD
{ {
configOK=0; configOK=0;
} }
ccpmYawErrorLast = 0.0;
} }
else else
{//TODO need to implement the stabilization PID loop to provide heading hold gyro functionality in manual control mode {//TODO need to implement the stabilization PID loop to provide heading hold gyro functionality in manual control mode
//currently just coppied from simple stabilisation
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
if (desired->Yaw<0)
{
desiredYaw = 360 + (desired->Yaw*180.0);
}
else
{
desiredYaw = (desired->Yaw*180.0);
}
// Yaw stabilization control loop
ccpmYawError = desiredYaw - attitudeActual.Yaw;
//this should make it take the quickest path to reach the desired yaw
if (ccpmYawError>180.0)ccpmYawError -= 360;
if (ccpmYawError<-180.0)ccpmYawError += 360;
ccpmYawDerivative = ccpmYawError - ccpmYawErrorLast;
ccpmYawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
ccpmYawIntegral = bound(ccpmYawIntegral+ccpmYawError*stabSettings.UpdatePeriod, -ccpmYawIntegralLimit, ccpmYawIntegralLimit);
desiredYaw = stabSettings.YawKp*ccpmYawError + stabSettings.YawKi*ccpmYawIntegral + stabSettings.YawKd*ccpmYawDerivative;;
desiredYaw = bound(desiredYaw, -1.0, 1.0);
ccpmYawErrorLast = ccpmYawError;
if ( settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE )
{
cmd->Channel[ settings->CCPMTailRotor ] = scaleChannel(desiredYaw, settings->ChannelMax[ settings->CCPMTailRotor ],
settings->ChannelMin[ settings->CCPMTailRotor ],
settings->ChannelNeutral[ settings->CCPMTailRotor ]);
}
else
{
configOK=0;
}
} }
if (configOK<2)return -1; if (configOK<2)return -1;
return 0; return 0;