diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index f70d4c891..b2ab9a6c7 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -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 */ /** * @} diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c index 572055981..8aa3589e1 100644 --- a/flight/Libraries/CoordinateConversions.c +++ b/flight/Libraries/CoordinateConversions.c @@ -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]; +} diff --git a/flight/Libraries/inc/CoordinateConversions.h b/flight/Libraries/inc/CoordinateConversions.h index 3278e5dfe..7c2434eec 100644 --- a/flight/Libraries/inc/CoordinateConversions.h +++ b/flight/Libraries/inc/CoordinateConversions.h @@ -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_ diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 15caba190..42750251b 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -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 diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index f02739240..02da66b5f 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -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; diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 0eec0ab22..eff2daca2 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -85,6 +85,10 @@ #define CPULOAD_LIMIT_WARNING 80 #define CPULOAD_LIMIT_CRITICAL 95 +/* Stabilization options */ +#define PIOS_QUATERNION_STABILIZATION + + #endif /* PIOS_CONFIG_H */ /** * @} diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index eaf3b4f89..476a63ae5 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \