/** * Saves the data from the aircraft in one of six positions. * This is called when they press "save position" and starts * averaging data for this position. */ void SixPointCalibrationModel::savePositionData() { QMutexLocker lock(&sensorsUpdateLock); savePositionEnabledChanged(false); accel_accum_x.clear(); accel_accum_y.clear(); accel_accum_z.clear(); mag_accum_x.clear(); mag_accum_y.clear(); mag_accum_z.clear(); aux_mag_accum_x.clear(); aux_mag_accum_y.clear(); aux_mag_accum_z.clear(); collectingData = true; if (calibratingMag) { #ifdef FITTING_USING_CONTINOUS_ACQUISITION // Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit. if (!position) { connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); } #endif // FITTING_USING_CONTINOUS_ACQUISITION connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); connect(auxMagSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); }
/** Updates the accel bias raw values */ void LevelCalibrationModel::getSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); switch (obj->getObjID()) { case AttitudeState::OBJID: { AttitudeState::DataFields attitudeStateData = attitudeState->getData(); rot_accum_roll.append(attitudeStateData.Roll); rot_accum_pitch.append(attitudeStateData.Pitch); break; } default: Q_ASSERT(0); } // Work out the progress based on whichever has less double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES; progressChanged(p1 * 100); if (rot_accum_roll.size() >= LEVEL_SAMPLES && collectingData == true) { collectingData = false; AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); position++; switch (position) { case 1: rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll); displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and press Save Position..."), WizardModel::Prompt); displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD); savePositionEnabledChanged(true); break; case 2: rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); rot_data_pitch /= 2; rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll); rot_data_roll /= 2; attitudeState->setMetadata(memento.attitudeStateMdata); m_dirty = true; stopped(); displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY); displayInstructions(tr("Board level calibration completed successfully."), WizardModel::Success); break; } }
void LevelCalibrationModel::savePosition() { QMutexLocker lock(&sensorsUpdateLock); savePositionEnabledChanged(false); rot_accum_pitch.clear(); rot_accum_roll.clear(); collectingData = true; connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); displayInstructions(tr("Hold...")); }
/** * Starts an accelerometer bias calibration. */ void LevelCalibrationModel::start() { memento.attitudeStateMdata = attitudeState->getMetadata(); UAVObject::Metadata mdata = attitudeState->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; attitudeState->setMetadata(mdata); rot_data_pitch = 0; rot_data_roll = 0; // reset dirty state to forget previous unsaved runs m_dirty = false; position = 0; started(); // Show instructions and enable controls progressChanged(0); displayInstructions(tr("Place horizontally and press Save Position..."), WizardModel::Prompt); displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); savePositionEnabledChanged(true); }
/** * Called by the "Start" button. Sets up the meta data and enables the * buttons to perform six point calibration of the magnetometer (optionally * accel) to compute the scale and bias of this sensor based on the current * home location magnetic strength. */ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) { calibratingAccel = calibrateAccel; calibratingMag = calibrateMag; started(); // check if Homelocation is set HomeLocation::DataFields homeLocationData = homeLocation->getData(); if (!homeLocationData.Set) { displayInstructions(tr("Home location not set, please set your home location and retry."), WizardModel::Warn); displayInstructions(tr("Aborting calibration!"), WizardModel::Failure); stopped(); return; } // Store and reset board rotation before calibration starts storeAndClearBoardRotation(); // Calibration accel AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); memento.accelGyroSettingsData = accelGyroSettingsData; accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1; accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1; accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0; accelGyroSettings->setData(accelGyroSettingsData); // Calibration mag RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); memento.revoCalibrationData = revoCalibrationData; // Reset the transformation matrix to identity for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) { revoCalibrationData.mag_transform[i] = 0; } revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1; revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1; revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 1; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0; // Disable adaptive mag nulling revoCalibrationData.MagBiasNullingRate = 0; revoCalibration->setData(revoCalibrationData); // Calibration AuxMag AuxMagSettings::DataFields auxMagSettingsData = auxMagSettings->getData(); memento.auxMagSettings = auxMagSettingsData; // Reset the transformation matrix to identity for (int i = 0; i < AuxMagSettings::MAG_TRANSFORM_R2C2; i++) { auxMagSettingsData.mag_transform[i] = 0; } auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R0C0] = 1; auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R1C1] = 1; auxMagSettingsData.mag_transform[AuxMagSettings::MAG_TRANSFORM_R2C2] = 1; auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_X] = 0; auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Y] = 0; auxMagSettingsData.mag_bias[AuxMagSettings::MAG_BIAS_Z] = 0; // Disable adaptive mag nulling auxMagSettingsData.MagBiasNullingRate = 0; auxMagSettings->setData(auxMagSettingsData); QThread::usleep(100000); mag_accum_x.clear(); mag_accum_y.clear(); mag_accum_z.clear(); mag_fit_x.clear(); mag_fit_y.clear(); mag_fit_z.clear(); // Need to get as many accel updates as possible memento.accelStateMetadata = accelState->getMetadata(); if (calibrateAccel) { UAVObject::Metadata mdata = accelState->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; accelState->setMetadata(mdata); } // Need to get as many mag updates as possible memento.magSensorMetadata = magSensor->getMetadata(); memento.auxMagSensorMetadata = auxMagSensor->getMetadata(); if (calibrateMag) { UAVObject::Metadata mdata = magSensor->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; magSensor->setMetadata(mdata); mdata = auxMagSensor->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; auxMagSensor->setMetadata(mdata); } // reset dirty state to forget previous unsaved runs m_dirty = false; if (calibrateMag) { currentSteps = &calibrationStepsMag; } else { currentSteps = &calibrationStepsAccelOnly; } position = 0; // Show instructions and enable controls progressChanged(0); displayInstructions((*currentSteps)[0].instructions, WizardModel::Prompt); showHelp((*currentSteps)[0].visualHelp); savePositionEnabledChanged(true); }