rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

AHRS: Make mag variance account for the vector length of magnetic field. Also

Browse Source 
moved around where the axis are swapped and made the negative sign come from
calibration.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1931 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-10 00:46:01 +00:00 committed by peabody124
parent fc0f0761a0
commit e72b6051b5
2 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
flight/AHRS/ahrs.c
View File

@ -191,7 +191,6 @@ static uint8_t adc_oversampling = 20;
/** /**
* @brief AHRS Main function * @brief AHRS Main function
*/ */
float mag[3];
int main() int main()
{ {
float gyro[3], accel[3]; float gyro[3], accel[3];
@ -319,8 +318,9 @@ for all data to be up to date before doing anything*/
// Get magnetic readings // Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) { if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis); PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0]; // Swap the axis here to acount for orientation of mag chip (notice 0 and 1 swapped in raw)
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1]; mag_data.scaled.axis[0] = (mag_data.raw.axis[1] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
mag_data.scaled.axis[1] = (mag_data.raw.axis[0] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2]; mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
mag_data.updated = 1; mag_data.updated = 1;
} }
@ -352,11 +352,6 @@ for all data to be up to date before doing anything*/
accel[0] = accel_data.filtered.x, accel[0] = accel_data.filtered.x,
accel[1] = accel_data.filtered.y, accel[1] = accel_data.filtered.y,
accel[2] = accel_data.filtered.z, accel[2] = accel_data.filtered.z,
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
// (left, backward, up). Remapping to (forward, right, down).
mag[0] = -mag_data.scaled.axis[1];
mag[1] = -mag_data.scaled.axis[0];
mag[2] = -mag_data.scaled.axis[2];
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE); INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
send_attitude(); // get message out quickly send_attitude(); // get message out quickly
@ -374,13 +369,14 @@ for all data to be up to date before doing anything*/
sin(gps_data.heading * M_PI / 180); sin(gps_data.heading * M_PI / 180);
INSSetPosVelVar(0.004); INSSetPosVelVar(0.004);
if (gps_data.updated) { if (mag_data.updated) {
//TOOD: add check for altitude updates //TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED, FullCorrection(mag_data.scaled.axis,
gps_data.NED,
vel, vel,
altitude_data. altitude_data.
altitude); altitude);
gps_data.updated = 0; mag_data.updated = 0;
} else { } else {
GpsBaroCorrection(gps_data.NED, GpsBaroCorrection(gps_data.NED,
vel, vel,
@ -389,10 +385,9 @@ for all data to be up to date before doing anything*/
} }
gps_data.updated = false; gps_data.updated = false;
mag_data.updated = 0;
} else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR } else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR
&& mag_data.updated == 1) { && mag_data.updated == 1) {
MagCorrection(mag); // only trust mags if outdoors MagCorrection(mag_data.scaled.axis); // only trust mags if outdoors
mag_data.updated = 0; mag_data.updated = 0;
} else { } else {
// Indoors, update with zero position and velocity and high covariance // Indoors, update with zero position and velocity and high covariance
@ -404,7 +399,8 @@ for all data to be up to date before doing anything*/
vel[2] = 0; vel[2] = 0;
if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) { if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors MagVelBaroCorrection(mag_data.scaled.axis,
vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0; mag_data.updated = 0;
} else { } else {
VelBaroCorrection(vel, altitude_data.altitude); VelBaroCorrection(vel, altitude_data.altitude);
@ -813,7 +809,11 @@ void calibration_callback(AhrsObjHandle obj)
mag_data.calibration.scale[ct] = cal.mag_scale[ct]; mag_data.calibration.scale[ct] = cal.mag_scale[ct];
mag_data.calibration.variance[ct] = cal.mag_var[ct]; mag_data.calibration.variance[ct] = cal.mag_var[ct];
} }
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]}; // Note: We need the divided by 1000^2 since we scale mags to have a norm of 1000 and they are scaled to
// one in code
float mag_var[3] = {mag_data.calibration.variance[0] / 1000 / 1000,
mag_data.calibration.variance[1] / 1000 / 1000,
mag_data.calibration.variance[2] / 1000 / 1000};
INSSetMagVar(mag_var); INSSetMagVar(mag_var);
INSSetAccelVar(accel_data.calibration.variance); INSSetAccelVar(accel_data.calibration.variance);
INSSetGyroVar(gyro_data.calibration.variance); INSSetGyroVar(gyro_data.calibration.variance);

18
ground/src/plugins/config/configahrswidget.cpp
View File

@ -518,7 +518,7 @@ void ConfigAHRSWidget::computeScaleBias()
field->setDouble(-listMean(gyro_accum_x) * M_PI / 180.0f,0); field->setDouble(-listMean(gyro_accum_x) * M_PI / 180.0f,0);
field->setDouble(-listMean(gyro_accum_y) * M_PI / 180.0f,1); field->setDouble(-listMean(gyro_accum_y) * M_PI / 180.0f,1);
field->setDouble(-listMean(gyro_accum_z) * M_PI / 180.0f,2); field->setDouble(-listMean(gyro_accum_z) * M_PI / 180.0f,2);
qDebug() << listMean(gyro_accum_x) * M_PI / 180.0f << listMean(gyro_accum_y) * M_PI / 180.0f << listMean(gyro_accum_x) * M_PI / 180.0f;
field = obj->getField(QString("accel_scale")); field = obj->getField(QString("accel_scale"));
field->setDouble(sign(S[0]) * S[0],0); field->setDouble(sign(S[0]) * S[0],0);
field->setDouble(sign(S[1]) * S[1],1); field->setDouble(sign(S[1]) * S[1],1);
@ -531,14 +531,14 @@ qDebug() << listMean(gyro_accum_x) * M_PI / 180.0f << listMean(gyro_accum_y) * M
SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b); SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b);
field = obj->getField(QString("mag_scale")); field = obj->getField(QString("mag_scale"));
field->setDouble(sign(S[0]) * S[0],0); field->setDouble(-sign(S[0]) * S[0],0);
field->setDouble(sign(S[1]) * S[1],1); field->setDouble(-sign(S[1]) * S[1],1);
field->setDouble(sign(S[2]) * S[2],2); field->setDouble(-sign(S[2]) * S[2],2);
field = obj->getField(QString("mag_bias")); field = obj->getField(QString("mag_bias"));
field->setDouble(sign(S[0]) * b[0], 0); field->setDouble(-sign(S[0]) * b[0], 0);
field->setDouble(sign(S[1]) * b[1], 1); field->setDouble(-sign(S[1]) * b[1], 1);
field->setDouble(sign(S[2]) * b[2], 2); field->setDouble(-sign(S[2]) * b[2], 2);
obj->updated(); obj->updated();
@ -582,6 +582,8 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
obj->updated(); obj->updated();
usleep(100000);
gyro_accum_x.clear(); gyro_accum_x.clear();
gyro_accum_y.clear(); gyro_accum_y.clear();
gyro_accum_z.clear(); gyro_accum_z.clear();
@ -591,7 +593,7 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
initialMdata = obj->getMetadata(); initialMdata = obj->getMetadata();
UAVObject::Metadata mdata = initialMdata; UAVObject::Metadata mdata = initialMdata;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = 50; mdata.flightTelemetryUpdatePeriod = 100;
obj->setMetadata(mdata); obj->setMetadata(mdata);
/* Show instructions and enable controls */ /* Show instructions and enable controls */