rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/OP-1358_add_separate_rotation_calibration_offset_revo_only' into rel-14.06

Browse Source
This commit is contained in:
Alessio Morale 2014-07-24 23:47:14 +02:00
commit e5b52d46d9
3 changed files with 25 additions and 9 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);
}
/**

4
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp
View File

@ -159,8 +159,8 @@ void LevelCalibrationModel::save()
// Update the biases based on collected data
// "rotate" the board in the opposite direction as the calculated offset
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] -= rot_data_pitch;
attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] -= rot_data_roll;
attitudeSettings->setData(attitudeSettingsData);

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"/>