/**
* 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);
}
