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

OP-313 Quaternion based stabilization, gets rid of lots of checking angle

Browse Source 
signs, should be stable for all orientations

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2932 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-03-02 01:25:34 +00:00 committed by peabody124
parent 3488d28a5b
commit 8930ec01de
7 changed files with 67 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/CopterControl/System/inc/pios_config.h
View File

@ -90,9 +90,11 @@
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 644
#define PIOS_SYSTEM_STACK_SIZE 644
#define PIOS_STABILIZATION_STACK_SIZE 544
#define PIOS_STABILIZATION_STACK_SIZE 624
#define PIOS_TELEM_STACK_SIZE 500
//#define PIOS_QUATERNION_STABILIZATION
#endif /* PIOS_CONFIG_H */
/**
* @}

26
flight/Libraries/CoordinateConversions.c
View File

@ -120,7 +120,7 @@ void RneFromLLA(double LLA[3], float Rne[3][3])
}
// ****** find roll, pitch, yaw from quaternion ********
void Quaternion2RPY(float q[4], float rpy[3])
void Quaternion2RPY(const float q[4], float rpy[3])
{
float R13, R11, R12, R23, R33;
float q0s = q[0] * q[0];
@ -142,7 +142,7 @@ void Quaternion2RPY(float q[4], float rpy[3])
}
// ****** find quaternion from roll, pitch, yaw ********
void RPY2Quaternion(float rpy[3], float q[4])
void RPY2Quaternion(const float rpy[3], float q[4])
{
float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi;
@ -348,3 +348,25 @@ float VectorMagnitude(const float v[3])
return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
void quat_inverse(float q[4])
{
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
void quat_copy(const float q[4], float qnew[4])
{
qnew[0] = q[0];
qnew[1] = q[1];
qnew[2] = q[2];
qnew[3] = q[3];
}
void quat_mult(const float q1[4], const float q2[4], float qout[4])
{
qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
qout[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}

7
flight/Libraries/inc/CoordinateConversions.h
View File

@ -39,10 +39,10 @@ uint16_t ECEF2LLA(double ECEF[3], double LLA[3]);
void RneFromLLA(double LLA[3], float Rne[3][3]);
// ****** find roll, pitch, yaw from quaternion ********
void Quaternion2RPY(float q[4], float rpy[3]);
void Quaternion2RPY(const float q[4], float rpy[3]);
// ****** find quaternion from roll, pitch, yaw ********
void RPY2Quaternion(float rpy[3], float q[4]);
void RPY2Quaternion(const float rpy[3], float q[4]);
//** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]);
@ -68,6 +68,9 @@ void CrossProduct(const float v1[3], const float v2[3], float result[3]);
// ****** Vector Magnitude ********
float VectorMagnitude(const float v[3]);
void quat_inverse(float q[4]);
void quat_copy(const float q[4], float qnew[4]);
void quat_mult(const float q1[4], const float q2[4], float qout[4]);
#endif // COORDINATECONVERSIONS_H_

4
flight/Modules/ManualControl/manualcontrol.c
View File

@ -486,12 +486,12 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
(stab_settings[0] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Roll = (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Pitch :
stabilization.Pitch = (stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Pitch :
(stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Pitch * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] :
(stab_settings[1] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Roll = (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Yaw :
stabilization.Yaw = (stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) ? cmd->Yaw :
(stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) ? cmd->Yaw * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] :
(stab_settings[2] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
0; // this is an invalid mode

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

@ -42,6 +42,7 @@
#include "manualcontrolcommand.h"
#include "systemsettings.h"
#include "ahrssettings.h"
#include "CoordinateConversions.h"
// Private constants
#define MAX_QUEUE_SIZE 1
@ -79,7 +80,7 @@ pid_type pids[PID_MAX];
// Private functions
static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float desired, const float actual, const uint8_t angular);
static float ApplyPid(pid_type * pid, const float err);
static float bound(float val);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev);
@ -154,9 +155,30 @@ static void stabilizationTask(void* parameters)
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float q_desired[4];
float q_curr[4];
float q_error[4];
float local_error[3];
// Compute stabilization error as (q_desired' * q_current)', convert to RPY
RPY2Quaternion(&stabDesired.Roll, q_desired);
quat_copy(&attitudeActual.q1, q_curr);
quat_inverse(q_desired);
quat_mult(q_desired, q_curr, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw};
local_error[2] = fmod(local_error[2] + 180, 360) - 180;
#endif
float *attitudeDesiredAxis = &stabDesired.Roll;
float *attitudeActualAxis = &attitudeActual.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
@ -170,7 +192,7 @@ static void stabilizationTask(void* parameters)
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], attitudeDesiredAxis[ct], attitudeActualAxis[ct], 1);
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], local_error[ct]);
break;
}
}
@ -197,7 +219,7 @@ static void stabilizationTask(void* parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
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]);
actuatorDesiredAxis[ct] = bound(command);
break;
}
@ -252,21 +274,8 @@ static void stabilizationTask(void* parameters)
}
}
float ApplyPid(pid_type * pid, const float desired, const float actual, const uint8_t angular)
float ApplyPid(pid_type * pid, const float err)
{
float err = desired - actual;
if(angular) //take shortest route to desired position
{
if(err > 180)
{
err -= 360;
}
if(err < -180)
{
err += 360;
}
}
float diff = (err - pid->lastErr);
pid->lastErr = err;
pid->iAccumulator += err * pid->i * dT;

4
flight/OpenPilot/System/inc/pios_config.h
View File

@ -85,6 +85,10 @@
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Stabilization options */
#define PIOS_QUATERNION_STABILIZATION
#endif /* PIOS_CONFIG_H */
/**
* @}

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -39,7 +39,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/ahrsstatus.h \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.h \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.h \
$$UAVOBJECT_SYNTHETICS/attitudedesired.h \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.h \
$$UAVOBJECT_SYNTHETICS/actuatorsettings.h \
$$UAVOBJECT_SYNTHETICS/actuatordesired.h \
$$UAVOBJECT_SYNTHETICS/actuatorcommand.h \
@ -84,7 +84,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationsettings.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolsettings.cpp \
$$UAVOBJECT_SYNTHETICS/manualcontrolcommand.cpp \
$$UAVOBJECT_SYNTHETICS/attitudedesired.cpp \
$$UAVOBJECT_SYNTHETICS/stabilizationdesired.cpp \
$$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \
$$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \
$$UAVOBJECT_SYNTHETICS/actuatorcommand.cpp \