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

Make the mag offset nulling convergence rate come from the UAVO. When it is

Browse Source 
set to zero nulling does not occur to allow us to still calibrate the sensors.
This commit is contained in:
James Cotton 2012-07-25 11:23:27 -05:00
parent 3b57b492d0
commit c587ceebfd
2 changed files with 23 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/Modules/Sensors/sensors.c
View File

@ -70,14 +70,16 @@
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI) #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
// Private types // Private types
// Private variables
static xTaskHandle sensorsTaskHandle;
// Private functions // Private functions
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent * objEv); static void settingsUpdatedCb(UAVObjEvent * objEv);
static void magOffsetEstimation(MagnetometerData *mag); static void magOffsetEstimation(MagnetometerData *mag);
// Private variables
static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
// These values are initialized by settings but can be updated by the attitude algorithm // These values are initialized by settings but can be updated by the attitude algorithm
static bool bias_correct_gyro = true; static bool bias_correct_gyro = true;
@ -400,8 +402,10 @@ static void SensorsTask(void *parameters)
mag.z = mags[2]; mag.z = mags[2];
} }
// Correct for mag bias and update // Correct for mag bias and update if the rate is non zero
if(cal.MagBiasNullingRate > 0)
magOffsetEstimation(&mag); magOffsetEstimation(&mag);
MagnetometerSet(&mag); MagnetometerSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw(); mag_update_time = PIOS_DELAY_GetRaw();
} }
@ -423,7 +427,6 @@ static void magOffsetEstimation(MagnetometerData *mag)
// Constants, to possibly go into a UAVO // Constants, to possibly go into a UAVO
static const int UPDATE_INTERVAL = 10; static const int UPDATE_INTERVAL = 10;
static const float MIN_NORM_DIFFERENCE = 5; static const float MIN_NORM_DIFFERENCE = 5;
static const float CONVERGENCE_RATE = 1.0f;
static unsigned int call_count = 0; static unsigned int call_count = 0;
static float B2[3] = {0, 0, 0}; static float B2[3] = {0, 0, 0};
@ -451,7 +454,7 @@ static void magOffsetEstimation(MagnetometerData *mag)
if (norm_diff > MIN_NORM_DIFFERENCE) { if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]); float norm_b1 = sqrtf(B1[0]*B1[0] + B1[1]*B1[1] + B1[2]*B1[2]);
float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]); float norm_b2 = sqrtf(B2[0]*B2[0] + B2[1]*B2[1] + B2[2]*B2[2]);
float scale = CONVERGENCE_RATE * (norm_b2 - norm_b1) / norm_diff; float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale}; float b_error[3] = {(B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale};
magBias.x += b_error[0]; magBias.x += b_error[0];
@ -470,7 +473,6 @@ static void magOffsetEstimation(MagnetometerData *mag)
* Locally cache some variables from the AtttitudeSettings object * Locally cache some variables from the AtttitudeSettings object
*/ */
static void settingsUpdatedCb(UAVObjEvent * objEv) { static void settingsUpdatedCb(UAVObjEvent * objEv) {
RevoCalibrationData cal;
RevoCalibrationGet(&cal); RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
@ -488,6 +490,15 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
// Do not store gyros_bias here as that comes from the state estimator and should be // Do not store gyros_bias here as that comes from the state estimator and should be
// used from GyroBias directly // used from GyroBias directly
// Zero out any adaptive tracking
MagBiasData magBias;
MagBiasGet(&magBias);
magBias.x = 0;
magBias.y = 0;
magBias.z = 0;
MagBiasSet(&magBias);
AttitudeSettingsData attitudeSettings; AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE); bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE);

6
shared/uavobjectdefinition/revocalibration.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="RevoCalibration" singleinstance="true" settings="true"> <object name="RevoCalibration" singleinstance="true" settings="true">
<description>Settings for the INS to control the algorithm and what is updated</description> <description>Settings for the INS to control the algorithm and what is updated</description>
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<!-- Sensor noises --> <!-- Sensor noises -->
<field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/> <field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
@ -18,6 +18,10 @@
<field name="gps_var" units="m^2" type="float" elementnames="Pos,Vel" defaultvalue="1,1"/> <field name="gps_var" units="m^2" type="float" elementnames="Pos,Vel" defaultvalue="1,1"/>
<field name="baro_var" units="m^2" type="float" elements="1" defaultvalue="1"/> <field name="baro_var" units="m^2" type="float" elements="1" defaultvalue="1"/>
<!-- These settings are related to how the sensors are post processed -->
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>