rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1309 Fixed bug in Quaternion Stabilization Error Calculation in regards to independent axis

Browse Source
This commit is contained in:
Corvus Corax 2014-05-07 22:12:01 +02:00
parent 3f5f77d878
commit 153d2fb786
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/modules/Stabilization/outerloop.c
View File

@ -114,9 +114,19 @@ static void stabilizationOuterloopTask()
float q_desired[4]; float q_desired[4];
float q_error[4]; float q_error[4];
rpy_desired[0] = stabilizationDesiredAxis[0]; for (t = 0; t < 3; t++) {
rpy_desired[1] = stabilizationDesiredAxis[1]; switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
rpy_desired[2] = stabilizationDesiredAxis[2]; case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
rpy_desired[t] = stabilizationDesiredAxis[t];
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
break;
}
}
RPY2Quaternion(rpy_desired, q_desired); RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired); quat_inverse(q_desired);

2
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -144,7 +144,7 @@
#define PIOS_GPS_SETS_HOMELOCATION #define PIOS_GPS_SETS_HOMELOCATION
/* Stabilization options */ /* Stabilization options */
// #define PIOS_QUATERNION_STABILIZATION #define PIOS_QUATERNION_STABILIZATION
/* Performance counters */ /* Performance counters */
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 8379692