From 013427b62c1118e87f8f618d47f0bbc1a12957f4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 24 Dec 2015 14:54:16 -0800 Subject: [PATCH] Reset compass offsets to previous if cal cancelled Also fixed problem with setup complete signaling after positive calibration completed --- src/AutoPilotPlugins/APM/APMCompassCal.cc | 44 +++++++++++++--------- src/AutoPilotPlugins/APM/APMCompassCal.h | 5 ++- .../APM/APMSensorsComponentController.cc | 2 +- 3 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.cc b/src/AutoPilotPlugins/APM/APMCompassCal.cc index 8b6c7e1..04299d3 100644 --- a/src/AutoPilotPlugins/APM/APMCompassCal.cc +++ b/src/AutoPilotPlugins/APM/APMCompassCal.cc @@ -31,6 +31,12 @@ const unsigned int CalWorkerThread::calibration_sides = 6; const unsigned int CalWorkerThread::calibration_total_points = 240; const unsigned int CalWorkerThread::calibraton_duration_seconds = CalWorkerThread::calibration_sides * 10; +const char* CalWorkerThread::rgCompassParams[3][4] = { + { "COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z", "COMPASS_DEV_ID" }, + { "COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z", "COMPASS_DEV_ID2" }, + { "COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z", "COMPASS_DEV_ID3" }, +}; + CalWorkerThread::CalWorkerThread(Vehicle* vehicle, QObject* parent) : QThread(parent) , _vehicle(vehicle) @@ -137,23 +143,17 @@ CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void) free(worker_data.z[cur_mag]); } - static const char* rgCompassOffsetParam[3][3] = { - { "COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z" }, - { "COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z" }, - { "COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z" }, - }; - AutoPilotPlugin* plugin = _vehicle->autopilotPlugin(); if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_maggetParameterFact(-1, offsetParam)->setRawValue(-sphere_x[cur_mag]); - offsetParam = rgCompassOffsetParam[cur_mag][1]; + offsetParam = rgCompassParams[cur_mag][1]; plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_y[cur_mag]); - offsetParam = rgCompassOffsetParam[cur_mag][2]; + offsetParam = rgCompassParams[cur_mag][2]; plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_z[cur_mag]); } } @@ -613,24 +613,21 @@ void APMCompassCal::startCalibration(void) _calWorkerThread = new CalWorkerThread(_vehicle, this); connect(_calWorkerThread, &CalWorkerThread::vehicleTextMessage, this, &APMCompassCal::vehicleTextMessage); - static const char* rgCompassOffsetParam[3][4] = { - { "COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z", "COMPASS_DEV_ID" }, - { "COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z", "COMPASS_DEV_ID2" }, - { "COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z", "COMPASS_DEV_ID3" }, - }; - // Clear the offset parameters so we get raw data AutoPilotPlugin* plugin = _vehicle->autopilotPlugin(); for (int i=0; i<3; i++) { _calWorkerThread->rgCompassAvailable[i] = true; - const char* deviceIdParam = rgCompassOffsetParam[i][3]; + const char* deviceIdParam = CalWorkerThread::rgCompassParams[i][3]; if (plugin->parameterExists(-1, deviceIdParam)) { if (plugin->getParameterFact(-1, deviceIdParam)->rawValue().toInt() > 0) { for (int j=0; j<3; j++) { - const char* offsetParam = rgCompassOffsetParam[i][j]; + const char* offsetParam = CalWorkerThread::rgCompassParams[i][j]; if (plugin->parameterExists(-1, offsetParam)) { - plugin->getParameterFact(-1, offsetParam)->setRawValue(0.0); + Fact* paramFact = plugin->getParameterFact(-1, offsetParam); + + _rgSavedCompassOffsets[i][j] = paramFact->rawValue().toFloat(); + paramFact->setRawValue(0.0); } else { _calWorkerThread->rgCompassAvailable[i] = false; } @@ -652,6 +649,17 @@ void APMCompassCal::cancelCalibration(void) { _stopCalibration(); + // Put the original offsets back + AutoPilotPlugin* plugin = _vehicle->autopilotPlugin(); + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + const char* offsetParam = CalWorkerThread::rgCompassParams[i][j]; + if (plugin->parameterExists(-1, offsetParam)) { + plugin->getParameterFact(-1, offsetParam)-> setRawValue(_rgSavedCompassOffsets[i][j]); + } + } + } + // Simulate a cancelled message _emitVehicleTextMessage("[cal] calibration cancelled"); } diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.h b/src/AutoPilotPlugins/APM/APMCompassCal.h index 8eb014f..a0c8060 100644 --- a/src/AutoPilotPlugins/APM/APMCompassCal.h +++ b/src/AutoPilotPlugins/APM/APMCompassCal.h @@ -54,6 +54,8 @@ public: mavlink_raw_imu_t lastRawImu; mavlink_scaled_imu_t rgLastScaledImu[max_mags]; + static const char* rgCompassParams[3][4]; + signals: void vehicleTextMessage(int vehicleId, int compId, int severity, QString text); @@ -135,8 +137,6 @@ private: bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration void* worker_data); ///< Opaque data passed to worker routine - bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count); - calibrate_return calibrate(void); calibrate_return mag_calibration_worker(detect_orientation_return orientation, void* data); unsigned progress_percentage(mag_worker_data_t* worker_data); @@ -174,6 +174,7 @@ private: Vehicle* _vehicle; CalWorkerThread* _calWorkerThread; + float _rgSavedCompassOffsets[3][3]; }; #endif diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 30c3f04..7fe9eca 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -74,7 +74,7 @@ APMSensorsComponentController::APMSensorsComponentController(void) : APMAutoPilotPlugin * apmPlugin = qobject_cast(_vehicle->autopilotPlugin()); _sensorsComponent = apmPlugin->sensorsComponent(); - connect(apmPlugin, &APMAutoPilotPlugin::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged); + connect(_sensorsComponent, &VehicleComponent::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged); } /// Appends the specified text to the status log area in the ui