From 6eb0fd2b3b045fa5b3df9e55bbc777641fab88e1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 21 Jul 2012 11:50:58 -0500 Subject: [PATCH] Validate the mag and accel calibration before setting to prevent block NAN on the FC side. --- .../src/plugins/config/configrevowidget.cpp | 61 +++++++++++++++++-- 1 file changed, 57 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 5364fbb44..e9d66cf9a 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -740,15 +740,68 @@ void ConfigRevoWidget::computeScaleBias() revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = -sign(S[1]) * b[1]; revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = -sign(S[2]) * b[2]; - revoCalibration->setData(revoCalibrationData); - position = -1; //set to run again #ifdef SIX_POINT_CAL_ACCEL - m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias..."); + bool good_calibration = true; + + // Check the mag calibration is good + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X]; + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y]; + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]; + + // Check the accel calibration is good + good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] == + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X]; + good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] == + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y]; + good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] == + revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z]; + good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] == + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X]; + good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] == + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y]; + good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] == + revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z]; + if (good_calibration) { + revoCalibration->setData(revoCalibrationData); + m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias..."); + } else { + revoCalibrationData = revoCalibration->getData(); + m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); + } #else - m_ui->sixPointCalibInstructions->append("Computed mag scale and bias..."); + bool good_calibration = true; + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X]; + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y]; + good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] == + revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]; + good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] == + revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]; + if (good_calibration) { + revoCalibration->setData(revoCalibrationData); + m_ui->sixPointCalibInstructions->append("Computed mag scale and bias..."); + } else { + revoCalibrationData = revoCalibration->getData(); + m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); + } #endif + position = -1; //set to run again } /**