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-1358 Revo:Implement roll/pitch trim settings separated from "nominal" board rotation settings

Browse Source
This commit is contained in:
Alessio Morale 2014-05-28 22:58:28 +02:00
parent a72ef1a178
commit 9bee143476
2 changed files with 23 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
flight/modules/Sensors/sensors.c
View File

@ -69,13 +69,13 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 2
#define ZERO_ROT_ANGLE 0.00001f
// Private types
// Private functions
static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv);
// static void magOffsetEstimation(MagSensorData *mag);
// Private variables
static xTaskHandle sensorsTaskHandle;
@ -461,20 +461,35 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation
if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f
&& fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f &&
fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) {
if (fabsf(attitudeSettings.BoardRotation.Roll) < ZERO_ROT_ANGLE
&& fabsf(attitudeSettings.BoardRotation.Pitch) < ZERO_ROT_ANGLE &&
fabsf(attitudeSettings.BoardRotation.Yaw) < ZERO_ROT_ANGLE) {
rotate = 0;
} else {
rotate = 1;
}
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
float rotationQuat[4];
RPY2Quaternion(rpy, rotationQuat);
if (fabsf(attitudeSettings.BoardLevelTrim.Roll) > ZERO_ROT_ANGLE ||
fabsf(attitudeSettings.BoardLevelTrim.Pitch) > ZERO_ROT_ANGLE) {
float trimQuat[4];
float sumQuat[4];
rotate = 1;
const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f };
RPY2Quaternion(trimRpy, trimQuat);
quat_mult(rotationQuat, trimQuat, sumQuat);
Quaternion2R(sumQuat, R);
} else {
Quaternion2R(rotationQuat, R);
}
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
}
/**

1
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -2,6 +2,7 @@
<object name="AttitudeSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="BoardRotation" units="deg" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="BoardLevelTrim" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,0"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0.000001"/>