diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 167a06a..15c1ba3 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -560,6 +560,7 @@ HEADERS+= \
src/AutoPilotPlugins/APM/APMAirframeComponent.h \
src/AutoPilotPlugins/APM/APMAirframeComponentController.h \
src/AutoPilotPlugins/APM/APMAirframeComponentAirframes.h \
+ src/AutoPilotPlugins/APM/APMCompassCal.h \
src/AutoPilotPlugins/APM/APMComponent.h \
src/AutoPilotPlugins/APM/APMFlightModesComponent.h \
src/AutoPilotPlugins/APM/APMFlightModesComponentController.h \
@@ -613,6 +614,7 @@ SOURCES += \
src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc \
src/AutoPilotPlugins/APM/APMAirframeComponent.cc \
src/AutoPilotPlugins/APM/APMAirframeComponentController.cc \
+ src/AutoPilotPlugins/APM/APMCompassCal.cc \
src/AutoPilotPlugins/APM/APMComponent.cc \
src/AutoPilotPlugins/APM/APMFlightModesComponent.cc \
src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc \
diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.cc b/src/AutoPilotPlugins/APM/APMCompassCal.cc
new file mode 100644
index 0000000..f5072c0
--- /dev/null
+++ b/src/AutoPilotPlugins/APM/APMCompassCal.cc
@@ -0,0 +1,737 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+#include "APMCompassCal.h"
+#include "AutoPilotPlugin.h"
+
+QGC_LOGGING_CATEGORY(APMCompassCalLog, "APMCompassCalLog")
+
+const float CalWorkerThread::mag_sphere_radius = 0.2f;
+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;
+
+CalWorkerThread::CalWorkerThread(Vehicle* vehicle, QObject* parent)
+ : QThread(parent)
+ , _vehicle(vehicle)
+ , _cancel(false)
+{
+
+}
+
+void CalWorkerThread::run(void)
+{
+ if (calibrate() == calibrate_return_ok) {
+ _emitVehicleTextMessage(QStringLiteral("[cal] progress <100>"));
+ _emitVehicleTextMessage(QStringLiteral("[cal] calibration done: mag"));
+ }
+}
+
+void CalWorkerThread::_emitVehicleTextMessage(const QString& message)
+{
+ emit vehicleTextMessage(_vehicle->id(), 0, MAV_SEVERITY_INFO, message);
+ qDebug() << message;
+}
+
+unsigned CalWorkerThread::progress_percentage(mag_worker_data_t* worker_data)
+{
+ return 100 * ((float)worker_data->done_count) / calibration_sides;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void)
+{
+ calibrate_return result = calibrate_return_ok;
+
+ mag_worker_data_t worker_data;
+
+ worker_data.done_count = 0;
+ worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
+ worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
+ worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
+
+ // Collect data for all sides
+ worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
+
+ for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+ // Initialize to no memory allocated
+ worker_data.x[cur_mag] = NULL;
+ worker_data.y[cur_mag] = NULL;
+ worker_data.z[cur_mag] = NULL;
+ worker_data.calibration_counter_total[cur_mag] = 0;
+ }
+
+ const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
+
+ for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+ if (rgCompassAvailable[cur_mag]) {
+ worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+ worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+ worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+ if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
+ _emitVehicleTextMessage("[cal] ERROR: out of memory");
+ result = calibrate_return_error;
+ }
+ }
+ }
+
+ if (result == calibrate_return_ok) {
+ result = calibrate_from_orientation(
+ worker_data.side_data_collected, // Sides to calibrate
+ &worker_data); // Opaque data for calibration worked
+ }
+
+ // Calculate calibration values for each mag
+
+ float sphere_x[max_mags];
+ float sphere_y[max_mags];
+ float sphere_z[max_mags];
+ float sphere_radius[max_mags];
+
+ // Sphere fit the data to get calibration values
+ if (result == calibrate_return_ok) {
+ for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+ if (rgCompassAvailable[cur_mag]) {
+ sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+ worker_data.calibration_counter_total[cur_mag],
+ 100, 0.0f,
+ &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
+ &sphere_radius[cur_mag]);
+
+ if (qIsNaN(sphere_x[cur_mag]) || qIsNaN(sphere_y[cur_mag]) || qIsNaN(sphere_z[cur_mag])) {
+ _emitVehicleTextMessage(QString("[cal] ERROR: NaN in sphere fit for mag %1").arg(cur_mag));
+ result = calibrate_return_error;
+ }
+ }
+ }
+ }
+
+ // Data points are no longer needed
+ for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+ free(worker_data.x[cur_mag]);
+ free(worker_data.y[cur_mag]);
+ 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_mag<max_mags; cur_mag++) {
+ if (rgCompassAvailable[cur_mag]) {
+ _emitVehicleTextMessage(QString("[cal] mag #%1 off: x:%2 y:%3 z:%4").arg(cur_mag).arg(-sphere_x[cur_mag]).arg(-sphere_y[cur_mag]).arg(-sphere_z[cur_mag]));
+
+ const char* offsetParam = rgCompassOffsetParam[cur_mag][0];
+ plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_x[cur_mag]);
+ offsetParam = rgCompassOffsetParam[cur_mag][1];
+ plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_y[cur_mag]);
+ offsetParam = rgCompassOffsetParam[cur_mag][2];
+ plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_z[cur_mag]);
+ }
+ }
+ }
+
+ return result;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::mag_calibration_worker(detect_orientation_return orientation, void* data)
+{
+ calibrate_return result = calibrate_return_ok;
+
+ unsigned int calibration_counter_side;
+
+ mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
+
+ _emitVehicleTextMessage("[cal] Rotate vehicle around the detected orientation");
+ _emitVehicleTextMessage(QString("[cal] Continue rotation for %1 seconds").arg(worker_data->calibration_interval_perside_seconds));
+
+ uint64_t calibration_deadline = QGC::groundTimeUsecs() + worker_data->calibration_interval_perside_useconds;
+
+ unsigned int loop_interval_usecs = (worker_data->calibration_interval_perside_seconds * 1000000) / worker_data->calibration_points_perside;
+
+ calibration_counter_side = 0;
+
+ while (QGC::groundTimeUsecs() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) {
+ if (_cancel) {
+ result = calibrate_return_cancelled;
+ break;
+ }
+
+ int prev_count[max_mags];
+ bool rejected = false;
+
+ for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+ prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+
+ if (!rgCompassAvailable[cur_mag]) {
+ continue;
+ }
+
+ lastScaledImuMutex.lock();
+ mavlink_scaled_imu_t copyLastScaledImu = rgLastScaledImu[cur_mag];
+ lastScaledImuMutex.unlock();
+
+#if 0
+ // Check if this measurement is good to go in
+ rejected = rejected || reject_sample(copyLastScaledImu.xmag, copyLastScaledImu.ymag, copyLastScaledImu.zmag,
+ worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
+ worker_data->calibration_counter_total[cur_mag],
+ calibration_sides * worker_data->calibration_points_perside);
+#endif
+
+ worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.xmag;
+ worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.ymag;
+ worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.zmag;
+ worker_data->calibration_counter_total[cur_mag]++;
+ }
+
+ // Keep calibration of all mags in lockstep
+ if (rejected) {
+ qCDebug(APMCompassCalLog) << QStringLiteral("Point rejected");
+
+ // Reset counts, since one of the mags rejected the measurement
+ for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+ worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
+ }
+ } else {
+ calibration_counter_side++;
+
+ // Progress indicator for side
+ _emitVehicleTextMessage(QString("[cal] %1 side calibration: progress <%2>").arg(detect_orientation_str(orientation)).arg(progress_percentage(worker_data) +
+ (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))));
+ }
+
+ usleep(loop_interval_usecs);
+ }
+
+ if (result == calibrate_return_ok) {
+ _emitVehicleTextMessage(QString("[cal] %1 side done, rotate to a different side").arg(detect_orientation_str(orientation)));
+
+ worker_data->done_count++;
+ _emitVehicleTextMessage(QString("[cal] progress <%1>").arg(progress_percentage(worker_data)));
+ }
+
+ return result;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::calibrate_from_orientation(
+ bool side_data_collected[detect_orientation_side_count],
+ void* worker_data)
+{
+ calibrate_return result = calibrate_return_ok;
+
+ unsigned orientation_failures = 0;
+
+ // Rotate through all requested orientations
+ while (true) {
+ if (_cancel) {
+ result = calibrate_return_cancelled;
+ break;
+ }
+
+ unsigned int side_complete_count = 0;
+
+ // Update the number of completed sides
+ for (unsigned i = 0; i < detect_orientation_side_count; i++) {
+ if (side_data_collected[i]) {
+ side_complete_count++;
+ }
+ }
+
+ if (side_complete_count == detect_orientation_side_count) {
+ // We have completed all sides, move on
+ break;
+ }
+
+ /* inform user which orientations are still needed */
+ char pendingStr[256];
+ pendingStr[0] = 0;
+
+ for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
+ if (!side_data_collected[cur_orientation]) {
+ strcat(pendingStr, " ");
+ strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
+ }
+ }
+ _emitVehicleTextMessage(QString("[cal] pending:%1").arg(pendingStr));
+
+ _emitVehicleTextMessage(QStringLiteral("[cal] hold vehicle still on a pending side"));
+ enum detect_orientation_return orient = detect_orientation();
+
+ if (orient == DETECT_ORIENTATION_ERROR) {
+ orientation_failures++;
+ _emitVehicleTextMessage(QStringLiteral("[cal] detected motion, hold still..."));
+ continue;
+ }
+
+ /* inform user about already handled side */
+ if (side_data_collected[orient]) {
+ orientation_failures++;
+ _emitVehicleTextMessage(QString("%1 side already completed").arg(detect_orientation_str(orient)));
+ _emitVehicleTextMessage(QStringLiteral("rotate to a pending side"));
+ continue;
+ }
+
+ _emitVehicleTextMessage(QString("[cal] %1 orientation detected").arg(detect_orientation_str(orient)));
+ orientation_failures = 0;
+
+ // Call worker routine
+ result = mag_calibration_worker(orient, worker_data);
+ if (result != calibrate_return_ok ) {
+ break;
+ }
+
+ _emitVehicleTextMessage(QString("[cal] %1 side done, rotate to a different side").arg(detect_orientation_str(orient)));
+
+ // Note that this side is complete
+ side_data_collected[orient] = true;
+ usleep(200000);
+ }
+
+ return result;
+}
+
+enum CalWorkerThread::detect_orientation_return CalWorkerThread::detect_orientation(void)
+{
+ bool stillDetected = false;
+ quint64 stillDetectTime;
+
+ int16_t lastX = 0;
+ int16_t lastY = 0;
+ int16_t lastZ = 0;
+
+ while (true) {
+ lastScaledImuMutex.lock();
+ mavlink_raw_imu_t copyLastRawImu = lastRawImu;
+ lastScaledImuMutex.unlock();
+
+ int16_t xDelta = abs(lastX - copyLastRawImu.xacc);
+ int16_t yDelta = abs(lastY - copyLastRawImu.yacc);
+ int16_t zDelta = abs(lastZ - copyLastRawImu.zacc);
+
+ lastX = copyLastRawImu.xacc;
+ lastY = copyLastRawImu.yacc;
+ lastZ = copyLastRawImu.zacc;
+
+ if (xDelta < 100 && yDelta < 100 && zDelta < 100) {
+ if (stillDetected) {
+ if (QGC::groundTimeMilliseconds() - stillDetectTime > 1000) {
+ break;
+ }
+ } else {
+ stillDetectTime = QGC::groundTimeMilliseconds();
+ stillDetected = true;
+ }
+ } else {
+ stillDetected = false;
+ }
+
+ if (_cancel) {
+ break;
+ }
+
+ // FIXME: No timeout for never detect still
+
+ usleep(1000);
+ }
+
+ static const uint16_t rawImuOneG = 800;
+ static const uint16_t rawImuNoGThreshold = 200;
+
+ if (lastX > rawImuOneG && abs(lastY) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ]
+ }
+
+ if (lastX < -rawImuOneG && abs(lastY) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ]
+ }
+
+ if (lastY > rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ]
+ }
+
+ if (lastY < -rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ]
+ }
+
+ if (lastZ > rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastY) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ]
+ }
+
+ if (lastZ < -rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastY) < rawImuNoGThreshold) {
+ return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
+ }
+
+ _emitVehicleTextMessage(QStringLiteral("[cal] ERROR: invalid orientation"));
+
+ return DETECT_ORIENTATION_ERROR; // Can't detect orientation
+}
+
+const char* CalWorkerThread::detect_orientation_str(enum detect_orientation_return orientation)
+{
+ static const char* rgOrientationStrs[] = {
+ "back", // tail down
+ "front", // nose down
+ "left",
+ "right",
+ "up", // upside-down
+ "down", // right-side up
+ "error"
+ };
+
+ return rgOrientationStrs[orientation];
+}
+
+#if 0
+bool CalWorkerThread::reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
+{
+ float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
+
+ for (size_t i = 0; i < count; i++) {
+ float dx = sx - x[i];
+ float dy = sy - y[i];
+ float dz = sz - z[i];
+ float dist = sqrtf(dx * dx + dy * dy + dz * dz);
+
+ qDebug() << dist << min_sample_dist;
+
+ if (dist < min_sample_dist) {
+ return true;
+ }
+ }
+
+ return false;
+}
+#endif
+
+int CalWorkerThread::sphere_fit_least_squares(const float x[], const float y[], const float z[],
+ unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
+ float *sphere_radius)
+{
+
+ float x_sumplain = 0.0f;
+ float x_sumsq = 0.0f;
+ float x_sumcube = 0.0f;
+
+ float y_sumplain = 0.0f;
+ float y_sumsq = 0.0f;
+ float y_sumcube = 0.0f;
+
+ float z_sumplain = 0.0f;
+ float z_sumsq = 0.0f;
+ float z_sumcube = 0.0f;
+
+ float xy_sum = 0.0f;
+ float xz_sum = 0.0f;
+ float yz_sum = 0.0f;
+
+ float x2y_sum = 0.0f;
+ float x2z_sum = 0.0f;
+ float y2x_sum = 0.0f;
+ float y2z_sum = 0.0f;
+ float z2x_sum = 0.0f;
+ float z2y_sum = 0.0f;
+
+ for (unsigned int i = 0; i < size; i++) {
+
+ float x2 = x[i] * x[i];
+ float y2 = y[i] * y[i];
+ float z2 = z[i] * z[i];
+
+ x_sumplain += x[i];
+ x_sumsq += x2;
+ x_sumcube += x2 * x[i];
+
+ y_sumplain += y[i];
+ y_sumsq += y2;
+ y_sumcube += y2 * y[i];
+
+ z_sumplain += z[i];
+ z_sumsq += z2;
+ z_sumcube += z2 * z[i];
+
+ xy_sum += x[i] * y[i];
+ xz_sum += x[i] * z[i];
+ yz_sum += y[i] * z[i];
+
+ x2y_sum += x2 * y[i];
+ x2z_sum += x2 * z[i];
+
+ y2x_sum += y2 * x[i];
+ y2z_sum += y2 * z[i];
+
+ z2x_sum += z2 * x[i];
+ z2y_sum += z2 * y[i];
+ }
+
+ //
+ //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
+ //
+ // P is a structure that has been computed with the data earlier.
+ // P.npoints is the number of elements; the length of X,Y,Z are identical.
+ // P's members are logically named.
+ //
+ // X[n] is the x component of point n
+ // Y[n] is the y component of point n
+ // Z[n] is the z component of point n
+ //
+ // A is the x coordiante of the sphere
+ // B is the y coordiante of the sphere
+ // C is the z coordiante of the sphere
+ // Rsq is the radius squared of the sphere.
+ //
+ //This method should converge; maybe 5-100 iterations or more.
+ //
+ float x_sum = x_sumplain / size; //sum( X[n] )
+ float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
+ float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
+ float y_sum = y_sumplain / size; //sum( Y[n] )
+ float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
+ float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
+ float z_sum = z_sumplain / size; //sum( Z[n] )
+ float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
+ float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
+
+ float XY = xy_sum / size; //sum( X[n] * Y[n] )
+ float XZ = xz_sum / size; //sum( X[n] * Z[n] )
+ float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
+ float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
+ float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
+ float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
+ float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
+ float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
+ float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
+
+ //Reduction of multiplications
+ float F0 = x_sum2 + y_sum2 + z_sum2;
+ float F1 = 0.5f * F0;
+ float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
+ float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
+ float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
+
+ //Set initial conditions:
+ float A = x_sum;
+ float B = y_sum;
+ float C = z_sum;
+
+ //First iteration computation:
+ float A2 = A * A;
+ float B2 = B * B;
+ float C2 = C * C;
+ float QS = A2 + B2 + C2;
+ float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
+
+ //Set initial conditions:
+ float Rsq = F0 + QB + QS;
+
+ //First iteration computation:
+ float Q0 = 0.5f * (QS - Rsq);
+ float Q1 = F1 + Q0;
+ float Q2 = 8.0f * (QS - Rsq + QB + F0);
+ float aA, aB, aC, nA, nB, nC, dA, dB, dC;
+
+ //Iterate N times, ignore stop condition.
+ unsigned int n = 0;
+
+#define FLT_EPSILON 1.1920929e-07F /* 1E-5 */
+
+ while (n < max_iterations) {
+ n++;
+
+ //Compute denominator:
+ aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
+ aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
+ aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
+ aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+ aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+ aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
+
+ //Compute next iteration
+ nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
+ nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
+ nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
+
+ //Check for stop condition
+ dA = (nA - A);
+ dB = (nB - B);
+ dC = (nC - C);
+
+ if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
+
+ //Compute next iteration's values
+ A = nA;
+ B = nB;
+ C = nC;
+ A2 = A * A;
+ B2 = B * B;
+ C2 = C * C;
+ QS = A2 + B2 + C2;
+ QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
+ Rsq = F0 + QB + QS;
+ Q0 = 0.5f * (QS - Rsq);
+ Q1 = F1 + Q0;
+ Q2 = 8.0f * (QS - Rsq + QB + F0);
+ }
+
+ *sphere_x = A;
+ *sphere_y = B;
+ *sphere_z = C;
+ *sphere_radius = sqrtf(Rsq);
+
+ return 0;
+}
+
+APMCompassCal::APMCompassCal(void)
+ : _vehicle(NULL)
+ , _calWorkerThread(NULL)
+{
+
+}
+
+APMCompassCal::~APMCompassCal()
+{
+
+}
+
+void APMCompassCal::setVehicle(Vehicle* vehicle)
+{
+ if (!vehicle) {
+ qWarning() << "vehicle == NULL";
+ }
+
+ _vehicle = vehicle;
+}
+
+void APMCompassCal::startCalibration(void)
+{
+ _setSensorTransmissionSpeed(true /* fast */);
+ connect (_vehicle, &Vehicle::mavlinkRawImu, this, &APMCompassCal::_handleMavlinkRawImu);
+ connect (_vehicle, &Vehicle::mavlinkScaledImu2, this, &APMCompassCal::_handleMavlinkScaledImu2);
+ connect (_vehicle, &Vehicle::mavlinkScaledImu3, this, &APMCompassCal::_handleMavlinkScaledImu3);
+
+ // Simulate a start message
+ _emitVehicleTextMessage("[cal] calibration started: mag");
+
+ _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];
+ 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];
+ if (plugin->parameterExists(-1, offsetParam)) {
+ plugin->getParameterFact(-1, offsetParam)->setRawValue(0.0);
+ } else {
+ _calWorkerThread->rgCompassAvailable[i] = false;
+ }
+ }
+ } else {
+ _calWorkerThread->rgCompassAvailable[i] = false;
+ }
+ } else {
+ _calWorkerThread->rgCompassAvailable[i] = false;
+ }
+
+ qCDebug(APMCompassCalLog) << QStringLiteral("Compass %1 available: %2").arg(i).arg(_calWorkerThread->rgCompassAvailable[i]);
+ }
+
+ _calWorkerThread->start();
+}
+
+void APMCompassCal::cancelCalibration(void)
+{
+ _stopCalibration();
+
+ // Simulate a cancelled message
+ _emitVehicleTextMessage("[cal] calibration cancelled");
+}
+
+void APMCompassCal::_handleMavlinkRawImu(mavlink_message_t message)
+{
+ _calWorkerThread->lastScaledImuMutex.lock();
+ mavlink_msg_raw_imu_decode(&message, &_calWorkerThread->lastRawImu);
+ _calWorkerThread->rgLastScaledImu[0].xacc = _calWorkerThread->lastRawImu.xacc;
+ _calWorkerThread->rgLastScaledImu[0].yacc = _calWorkerThread->lastRawImu.yacc;
+ _calWorkerThread->rgLastScaledImu[0].zacc = _calWorkerThread->lastRawImu.zacc;
+ _calWorkerThread->rgLastScaledImu[0].xgyro = _calWorkerThread->lastRawImu.xgyro;
+ _calWorkerThread->rgLastScaledImu[0].ygyro = _calWorkerThread->lastRawImu.ygyro;
+ _calWorkerThread->rgLastScaledImu[0].zgyro = _calWorkerThread->lastRawImu.zgyro;
+ _calWorkerThread->rgLastScaledImu[0].xmag = _calWorkerThread->lastRawImu.xmag;
+ _calWorkerThread->rgLastScaledImu[0].ymag = _calWorkerThread->lastRawImu.ymag;
+ _calWorkerThread->rgLastScaledImu[0].zmag = _calWorkerThread->lastRawImu.zmag;
+ _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_handleMavlinkScaledImu2(mavlink_message_t message)
+{
+ _calWorkerThread->lastScaledImuMutex.lock();
+ mavlink_msg_scaled_imu2_decode(&message, (mavlink_scaled_imu2_t*)&_calWorkerThread->rgLastScaledImu[1]);
+ qDebug() << "IMU2" << _calWorkerThread->rgLastScaledImu[1].xmag << _calWorkerThread->rgLastScaledImu[1].ymag << _calWorkerThread->rgLastScaledImu[1].zmag;
+ _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_handleMavlinkScaledImu3(mavlink_message_t message)
+{
+ _calWorkerThread->lastScaledImuMutex.lock();
+ mavlink_msg_scaled_imu3_decode(&message, (mavlink_scaled_imu3_t*)&_calWorkerThread->rgLastScaledImu[2]);
+ _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_setSensorTransmissionSpeed(bool fast)
+{
+ _vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, fast ? 10 : 2);
+}
+
+void APMCompassCal::_stopCalibration(void)
+{
+ _calWorkerThread->cancel();
+ disconnect (_vehicle, &Vehicle::mavlinkRawImu, this, &APMCompassCal::_handleMavlinkRawImu);
+ disconnect (_vehicle, &Vehicle::mavlinkScaledImu2, this, &APMCompassCal::_handleMavlinkScaledImu2);
+ disconnect (_vehicle, &Vehicle::mavlinkScaledImu3, this, &APMCompassCal::_handleMavlinkScaledImu3);
+ _setSensorTransmissionSpeed(false /* fast */);
+}
+
+void APMCompassCal::_emitVehicleTextMessage(const QString& message)
+{
+ emit vehicleTextMessage(_vehicle->id(), 0, MAV_SEVERITY_INFO, message);
+}
diff --git a/src/AutoPilotPlugins/APM/APMCompassCal.h b/src/AutoPilotPlugins/APM/APMCompassCal.h
new file mode 100644
index 0000000..8eb014f
--- /dev/null
+++ b/src/AutoPilotPlugins/APM/APMCompassCal.h
@@ -0,0 +1,180 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+ ======================================================================*/
+
+#ifndef APMCompassCal_H
+#define APMCompassCal_H
+
+#include <QObject>
+#include <QThread>
+#include <QVector3D>
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+#include "Vehicle.h"
+
+Q_DECLARE_LOGGING_CATEGORY(APMCompassCalLog)
+
+class CalWorkerThread : public QThread
+{
+ Q_OBJECT
+
+public:
+ CalWorkerThread(Vehicle* vehicle, QObject* parent = NULL);
+
+ // Cancel currently in progress calibration
+ void cancel(void) { _cancel = true; }
+
+ // Overrides from QThread
+ void run(void) Q_DECL_FINAL;
+
+ static const unsigned max_mags = 3;
+
+ bool rgCompassAvailable[max_mags];
+ QMutex lastScaledImuMutex;
+ mavlink_raw_imu_t lastRawImu;
+ mavlink_scaled_imu_t rgLastScaledImu[max_mags];
+
+signals:
+ void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
+
+private:
+ void _emitVehicleTextMessage(const QString& message);
+
+ // The routines below are based on the PX4 flight stack compass cal routines. Hence
+ // the PX4 Flight Stack coding style to maintain some level of code movement.
+
+ static const float mag_sphere_radius;
+ static const unsigned int calibration_sides; ///< The total number of sides
+ static const unsigned int calibration_total_points; ///< The total points per magnetometer
+ static const unsigned int calibraton_duration_seconds; ///< The total duration the routine is allowed to take
+
+ // The order of these cannot change since the calibration calculations depend on them in this order
+ enum detect_orientation_return {
+ DETECT_ORIENTATION_TAIL_DOWN,
+ DETECT_ORIENTATION_NOSE_DOWN,
+ DETECT_ORIENTATION_LEFT,
+ DETECT_ORIENTATION_RIGHT,
+ DETECT_ORIENTATION_UPSIDE_DOWN,
+ DETECT_ORIENTATION_RIGHTSIDE_UP,
+ DETECT_ORIENTATION_ERROR
+ };
+ static const unsigned detect_orientation_side_count = 6;
+
+ // Data passed to calibration worker routine
+ typedef struct {
+ unsigned done_count;
+ unsigned int calibration_points_perside;
+ unsigned int calibration_interval_perside_seconds;
+ uint64_t calibration_interval_perside_useconds;
+ unsigned int calibration_counter_total[max_mags];
+ bool side_data_collected[detect_orientation_side_count];
+ float* x[max_mags];
+ float* y[max_mags];
+ float* z[max_mags];
+ } mag_worker_data_t;
+
+ enum calibrate_return {
+ calibrate_return_ok,
+ calibrate_return_error,
+ calibrate_return_cancelled
+ };
+
+ /**
+ * Least-squares fit of a sphere to a set of points.
+ *
+ * Fits a sphere to a set of points on the sphere surface.
+ *
+ * @param x point coordinates on the X axis
+ * @param y point coordinates on the Y axis
+ * @param z point coordinates on the Z axis
+ * @param size number of points
+ * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
+ * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
+ * @param sphere_x coordinate of the sphere center on the X axis
+ * @param sphere_y coordinate of the sphere center on the Y axis
+ * @param sphere_z coordinate of the sphere center on the Z axis
+ * @param sphere_radius sphere radius
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
+ unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
+ float *sphere_radius);
+
+ /// Wait for vehicle to become still and detect it's orientation
+ /// @return Returns detect_orientation_return according to orientation of still vehicle
+ enum detect_orientation_return detect_orientation(void);
+
+ /// Returns the human readable string representation of the orientation
+ /// @param orientation Orientation to return string for, "error" if buffer is too small
+ const char* detect_orientation_str(enum detect_orientation_return orientation);
+
+ /// Perform calibration sequence which require a rest orientation detection prior to calibration.
+ /// @return OK: Calibration succeeded, ERROR: Calibration failed
+ calibrate_return calibrate_from_orientation(
+ 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);
+
+ Vehicle* _vehicle;
+ bool _cancel;
+};
+
+// Used to calibrate APM Stack compass by simulating PX4 Flight Stack firmware compass cal
+// on the ground station side of things.
+class APMCompassCal : public QObject
+{
+ Q_OBJECT
+
+public:
+ APMCompassCal(void);
+ ~APMCompassCal();
+
+ void setVehicle(Vehicle* vehicle);
+ void startCalibration(void);
+ void cancelCalibration(void);
+
+signals:
+ void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
+
+private slots:
+ void _handleMavlinkRawImu(mavlink_message_t message);
+ void _handleMavlinkScaledImu2(mavlink_message_t message);
+ void _handleMavlinkScaledImu3(mavlink_message_t message);
+
+private:
+ void _setSensorTransmissionSpeed(bool fast);
+ void _stopCalibration(void);
+ void _emitVehicleTextMessage(const QString& message);
+
+ Vehicle* _vehicle;
+ CalWorkerThread* _calWorkerThread;
+};
+
+#endif
+
diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml
index f5674de..e68c033 100644
--- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml
+++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml
@@ -40,8 +40,9 @@ QGCView {
// Help text which is shown both in the status text area prior to pressing a cal button and in the
// pre-calibration dialog.
- readonly property string boardRotationText: "If the autopilot is mounted in flight direction, leave the default value (None)"
- readonly property string compassRotationText: "If the compass or GPS module is mounted in flight direction, leave the default value (None)"
+ readonly property string orientationHelp: "If the orientation is in the direction of flight, select None."
+ readonly property string boardRotationText: "If the autopilot is mounted in flight direction, leave the default value (None)"
+ readonly property string compassRotationText: "If the compass or GPS module is mounted in flight direction, leave the default value (None)"
readonly property string compassHelp: "For Compass calibration you will need to rotate your vehicle through a number of positions. Most users prefer to do this wirelessly with the telemetry link."
readonly property string gyroHelp: "For Gyroscope calibration you will need to place your vehicle on a surface and leave it still."
@@ -61,64 +62,29 @@ QGCView {
readonly property int mainTextH1PointSize: ScreenTools.mediumFontPixelSize // Seems to be unused
readonly property int rotationColumnWidth: 250
- readonly property var rotations: [
- "None",
- "Yaw 45",
- "Yaw 90",
- "Yaw 135",
- "Yaw 180",
- "Yaw 225",
- "Yaw 270",
- "Yaw 315",
- "Roll 180",
- "Roll 180. Yaw 45",
- "Roll 180. Yaw 90",
- "Roll 180. Yaw 135",
- "Pitch 180",
- "Roll 180. Yaw 225",
- "Roll 180, Yaw 270",
- "Roll 180, Yaw 315",
- "Roll 90",
- "Roll 90, Yaw 45",
- "Roll 90, Yaw 90",
- "Roll 90, Yaw 135",
- "Roll 270",
- "Roll 270, Yaw 45",
- "Roll 270, Yaw 90",
- "Roll 270, Yaw 136",
- "Pitch 90",
- "Pitch 270",
- "Pitch 180, Yaw 90",
- "Pitch 180, Yaw 270",
- "Roll 90, Pitch 90",
- "Roll 180, Pitch 90",
- "Roll 270, Pitch 90",
- "Roll 90, Pitch 180",
- "Roll 270, Pitch 180",
- "Roll 90, Pitch 270",
- "Roll 180, Pitch 270",
- "Roll 270, Pitch 270",
- "Roll 90, Pitch 180, Yaw 90",
- "Roll 90, Yaw 270",
- "Yaw 293, Pitch 68, Roll 90",
- ]
-
- property Fact cal_mag0_id: controller.getParameterFact(-1, "COMPASS_DEV_ID")
- property Fact cal_mag1_id: controller.getParameterFact(-1, "COMPASS_DEV_ID2")
- property Fact cal_mag2_id: controller.getParameterFact(-1, "COMPASS_DEV_ID3")
- property Fact cal_mag0_rot: controller.getParameterFact(-1, "COMPASS_ORIENT")
- property Fact cal_mag1_rot: controller.getParameterFact(-1, "COMPASS_ORIENT2")
- property Fact cal_mag2_rot: controller.getParameterFact(-1, "COMPASS_ORIENT3")
-
- property Fact sens_board_rot: controller.getParameterFact(-1, "AHRS_ORIENTATION")
+
+ property Fact compass1Id: controller.getParameterFact(-1, "COMPASS_DEV_ID")
+ property Fact compass2Id: controller.getParameterFact(-1, "COMPASS_DEV_ID2")
+ property Fact compass3Id: controller.getParameterFact(-1, "COMPASS_DEV_ID3")
+ property Fact compass1External: controller.getParameterFact(-1, "COMPASS_EXTERNAL")
+ property Fact compass2External: controller.getParameterFact(-1, "COMPASS_EXTERN2")
+ property Fact compass3External: controller.getParameterFact(-1, "COMPASS_EXTERN3")
+ property Fact compass1Rot: controller.getParameterFact(-1, "COMPASS_ORIENT")
+ property Fact compass2Rot: controller.getParameterFact(-1, "COMPASS_ORIENT2")
+ property Fact compass3Rot: controller.getParameterFact(-1, "COMPASS_ORIENT3")
+ property Fact compass1Use: controller.getParameterFact(-1, "COMPASS_USE")
+ property Fact compass2Use: controller.getParameterFact(-1, "COMPASS_USE2")
+ property Fact compass3Use: controller.getParameterFact(-1, "COMPASS_USE3")
+
+ property Fact boardRot: controller.getParameterFact(-1, "AHRS_ORIENTATION")
property bool accelCalNeeded: controller.accelSetupNeeded
property bool compassCalNeeded: controller.compassSetupNeeded
// Id > = signals compass available, rot < 0 signals internal compass
- property bool showCompass0Rot: cal_mag0_id.value > 0 && cal_mag0_rot.value >= 0
- property bool showCompass1Rot: cal_mag1_id.value > 0 && cal_mag1_rot.value >= 0
- property bool showCompass2Rot: cal_mag2_id.value > 0 && cal_mag2_rot.value >= 0
+ property bool showCompass1Rot: compass1Id.value > 0 && compass1External.value != 0 && compass1Use.value != 0
+ property bool showCompass2Rot: compass2Id.value > 0 && compass2External.value != 0 && compass2Use.value != 0
+ property bool showCompass3Rot: compass3Id.value > 0 && compass3External.value != 0 && compass3Use.value != 0
APMSensorsComponentController {
id: controller
@@ -133,12 +99,6 @@ QGCView {
onResetStatusTextArea: statusLog.text = statusTextAreaDefaultText
- onSetCompassRotations: {
- if (showCompass0Rot || showCompass1Rot || showCompass2Rot) {
- showDialog(compassRotationDialogComponent, "Set Compass Rotation(s)", 50, StandardButton.Ok)
- }
- }
-
onWaitingForCancelChanged: {
if (controller.waitingForCancel) {
showMessage("Calibration Cancel", "Waiting for Vehicle to response to Cancel. This may take a few seconds.", 0)
@@ -157,393 +117,295 @@ QGCView {
id: preCalibrationDialog
function accept() {
- if (preCalibrationDialogType == "gyro") {
- controller.calibrateGyro()
- } else if (preCalibrationDialogType == "accel") {
+ if (preCalibrationDialogType == "accel") {
controller.calibrateAccel()
- } else if (preCalibrationDialogType == "level") {
- controller.calibrateLevel()
} else if (preCalibrationDialogType == "compass") {
controller.calibrateCompass()
- } else if (preCalibrationDialogType == "airspeed") {
- controller.calibrateAirspeed()
}
preCalibrationDialog.hideDialog()
}
- Column {
- anchors.fill: parent
- spacing: 5
-
- QGCLabel {
- width: parent.width
- wrapMode: Text.WordWrap
- text: preCalibrationDialogHelp
- }
+ QGCLabel {
+ id: label
+ anchors.left: parent.left
+ anchors.right: parent.right
+ wrapMode: Text.WordWrap
+ text: "Before calibrating make sure orientation settings are correct."
+ }
- QGCLabel {
- width: parent.width
- wrapMode: Text.WordWrap
- visible: (preCalibrationDialogType != "airspeed") && (preCalibrationDialogType != "gyro")
- text: boardRotationText
- }
+ Loader {
+ id: rotationsLoader
+ anchors.topMargin: ScreenTools.defaultFontPixelHeight
+ anchors.top: label.bottom
+ anchors.left: parent.left
+ anchors.right: parent.right
+ sourceComponent: rotationCombosComponent
- FactComboBox {
- width: rotationColumnWidth
- model: rotations
- visible: preCalibrationDialogType != "airspeed" && (preCalibrationDialogType != "gyro")
- fact: sens_board_rot
- }
+ property bool showCompassRotations: preCalibrationDialogType == "accel" ? false : true
}
}
}
Component {
- id: compassRotationDialogComponent
+ id: rotationCombosComponent
- QGCViewDialog {
- id: compassRotationDialog
+ Column {
+ QGCLabel {
+ font.pixelSize: sideBarH1PointSize
+ text: "Set Orientations"
+ }
- Column {
- anchors.fill: parent
- spacing: ScreenTools.defaultFontPixelHeight
+ Item {
+ width: 1
+ height: ScreenTools.defaultFontPixelHeight
+ }
- QGCLabel {
- width: parent.width
- wrapMode: Text.WordWrap
- text: compassRotationText
- }
+ QGCLabel {
+ width: parent.width
+ wrapMode: Text.WordWrap
+ text: orientationHelp
+ }
- // Compass 0 rotation
- Component {
- id: compass0ComponentLabel
+ Item {
+ width: 1
+ height: ScreenTools.defaultFontPixelHeight
+ }
- QGCLabel {
- font.pixelSize: sideBarH1PointSize
- text: "External Compass Orientation"
- }
+ // Board rotation
+ QGCLabel {
+ text: "Autopilot Orientation"
+ }
- }
- Component {
- id: compass0ComponentCombo
-
- FactComboBox {
- id: compass0RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag0_rot
- }
- }
- Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel : null }
- Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo : null }
- // Compass 1 rotation
- Component {
- id: compass1ComponentLabel
+ FactComboBox {
+ id: boardRotationCombo
+ width: parent.width
+ fact: boardRot
+ indexModel: false
+ }
- QGCLabel { text: "Compass 1 Orientation" }
- }
- Component {
- id: compass1ComponentCombo
-
- FactComboBox {
- id: compass1RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag1_rot
- }
- }
- Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel : null }
- Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo : null }
+ Item {
+ width: 1
+ height: ScreenTools.defaultFontPixelHeight
+ }
- // Compass 2 rotation
- Component {
- id: compass2ComponentLabel
+ // Compass 1 rotation
+ QGCLabel {
+ text: "Compass 1 Orientation"
+ visible: showCompassRotations && showCompass1Rot
+ }
- QGCLabel { text: "Compass 2 Orientation" }
- }
- Component {
- id: compass2ComponentCombo
-
- FactComboBox {
- id: compass1RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag2_rot
- }
- }
- Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel : null }
- Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo : null }
- } // Column
- } // QGCViewDialog
- } // Component - compassRotationDialogComponent
+ FactComboBox {
+ width: parent.width
+ fact: compass1Rot
+ indexModel: false
+ visible: showCompassRotations && showCompass1Rot
+ }
+
+ Item {
+ width: 1
+ height: ScreenTools.defaultFontPixelHeight
+ }
+
+ // Compass 2 rotation
+ QGCLabel {
+ text: "Compass 2 Orientation"
+ visible: showCompassRotations && showCompass2Rot
+ }
+
+ FactComboBox {
+ width: parent.width
+ fact: compass2Rot
+ indexModel: false
+ visible: showCompassRotations && showCompass2Rot
+ }
+
+ Item {
+ width: 1
+ height: ScreenTools.defaultFontPixelHeight
+ }
+
+ // Compass 3 rotation
+ QGCLabel {
+ text: "Compass 3 Orientation"
+ visible: showCompassRotations && showCompass3Rot
+ }
+
+ FactComboBox {
+ width: parent.width
+ fact: compass3Rot
+ indexModel: false
+ visible: showCompassRotations && showCompass3Rot
+ }
+ } // Column
+ } // Component - Rotation combos
QGCViewPanel {
id: panel
anchors.fill: parent
- Column {
- anchors.fill: parent
+ Row {
+ id: buttonsRow
+ spacing: ScreenTools.defaultFontPixelWidth
- Row {
- readonly property int buttonWidth: ScreenTools.defaultFontPixelWidth * 15
+ readonly property int buttonWidth: ScreenTools.defaultFontPixelWidth * 15
- spacing: ScreenTools.defaultFontPixelWidth
+ QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline }
- QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline }
+ IndicatorButton {
+ id: compassButton
+ width: parent.buttonWidth
+ text: "Compass"
+ indicatorGreen: !compassCalNeeded
- IndicatorButton {
- id: compassButton
- width: parent.buttonWidth
- text: "Compass - NYI"
- indicatorGreen: !compassCalNeeded
-
- onClicked: {
+ onClicked: {
+ if (controller.accelSetupNeeded) {
+ showMessage("Calibrate Compass", "Accelerometer must be calibrated prior to Compass.", StandardButton.Ok)
+ } else if (compass3Id.value != 0 && compass3Use.value !=0) {
+ showMessage("Unabled to calibrate", "Support for calibrating compass 3 is currently not supported by QGroundControl.", StandardButton.Ok)
+ } else {
preCalibrationDialogType = "compass"
preCalibrationDialogHelp = compassHelp
showDialog(preCalibrationDialogComponent, "Calibrate Compass", 50, StandardButton.Cancel | StandardButton.Ok)
}
}
+ }
- IndicatorButton {
- id: accelButton
- width: parent.buttonWidth
- text: "Accelerometer"
- indicatorGreen: !accelCalNeeded
-
- onClicked: {
- preCalibrationDialogType = "accel"
- preCalibrationDialogHelp = accelHelp
- showDialog(preCalibrationDialogComponent, "Calibrate Accelerometer", 50, StandardButton.Cancel | StandardButton.Ok)
- }
- }
- QGCButton {
- id: nextButton
- showBorder: true
- text: "Next"
- enabled: false
- onClicked: controller.nextClicked()
- }
+ IndicatorButton {
+ id: accelButton
+ width: parent.buttonWidth
+ text: "Accelerometer"
+ indicatorGreen: !accelCalNeeded
- QGCButton {
- id: cancelButton
- showBorder: true
- text: "Cancel"
- enabled: false
- onClicked: controller.cancelCalibration()
+ onClicked: {
+ preCalibrationDialogType = "accel"
+ preCalibrationDialogHelp = accelHelp
+ showDialog(preCalibrationDialogComponent, "Calibrate Accelerometer", 50, StandardButton.Cancel | StandardButton.Ok)
}
}
-
- Item { height: ScreenTools.defaultFontPixelHeight; width: 10 } // spacer
-
- ProgressBar {
- id: progressBar
- width: parent.width - rotationColumnWidth
+ QGCButton {
+ id: nextButton
+ showBorder: true
+ text: "Next"
+ enabled: false
+ onClicked: controller.nextClicked()
}
- Item { height: ScreenTools.defaultFontPixelHeight; width: 10 } // spacer
+ QGCButton {
+ id: cancelButton
+ showBorder: true
+ text: "Cancel"
+ enabled: false
+ onClicked: controller.cancelCalibration()
+ }
+ } // Row - Cal buttons
+
+ ProgressBar {
+ id: progressBar
+ anchors.topMargin: ScreenTools.defaultFontPixelHeight
+ anchors.top: buttonsRow.bottom
+ anchors.left: parent.left
+ anchors.right: rotationsLoader.left
+ }
- Item {
- property int calDisplayAreaWidth: parent.width - rotationColumnWidth
-
- width: parent.width
- height: parent.height - y
-
- TextArea {
- id: statusTextArea
- width: parent.calDisplayAreaWidth
- height: parent.height
- readOnly: true
- frameVisible: false
- text: statusTextAreaDefaultText
-
- style: TextAreaStyle {
- textColor: qgcPal.text
- backgroundColor: qgcPal.windowShade
- }
+ Item {
+ anchors.topMargin: ScreenTools.defaultFontPixelHeight
+ anchors.rightMargin: ScreenTools.defaultFontPixelHeight
+ anchors.top: progressBar.bottom
+ anchors.bottom: parent.bottom
+ anchors.left: parent.left
+ anchors.right: rotationsLoader.left
+
+ TextArea {
+ id: statusTextArea
+ anchors.fill: parent
+ readOnly: true
+ frameVisible: false
+ text: statusTextAreaDefaultText
+
+ style: TextAreaStyle {
+ textColor: qgcPal.text
+ backgroundColor: qgcPal.windowShade
}
+ }
- Rectangle {
- id: orientationCalArea
- width: parent.calDisplayAreaWidth
- height: parent.height
- visible: controller.showOrientationCalArea
- color: qgcPal.windowShade
-
- QGCLabel {
- id: orientationCalAreaHelpText
- anchors.margins: ScreenTools.defaultFontPixelWidth
- anchors.top: orientationCalArea.top
- anchors.left: orientationCalArea.left
- width: parent.width
- wrapMode: Text.WordWrap
- font.pixelSize: ScreenTools.mediumFontPixelSize
- }
+ Rectangle {
+ id: orientationCalArea
+ anchors.fill: parent
+ visible: controller.showOrientationCalArea
+ color: qgcPal.windowShade
- Flow {
- anchors.topMargin: ScreenTools.defaultFontPixelWidth
- anchors.top: orientationCalAreaHelpText.bottom
- anchors.left: orientationCalAreaHelpText.left
- width: parent.width
- height: parent.height - orientationCalAreaHelpText.implicitHeight
- spacing: ScreenTools.defaultFontPixelWidth
-
- VehicleRotationCal {
- visible: controller.orientationCalDownSideVisible
- calValid: controller.orientationCalDownSideDone
- calInProgress: controller.orientationCalDownSideInProgress
- calInProgressText: controller.orientationCalDownSideRotate ? "Rotate" : "Hold Still"
- imageSource: controller.orientationCalDownSideRotate ? "qrc:///qmlimages/VehicleDownRotate.png" : "qrc:///qmlimages/VehicleDown.png"
- }
- VehicleRotationCal {
- visible: controller.orientationCalUpsideDownSideVisible
- calValid: controller.orientationCalUpsideDownSideDone
- calInProgress: controller.orientationCalUpsideDownSideInProgress
- calInProgressText: controller.orientationCalUpsideDownSideRotate ? "Rotate" : "Hold Still"
- imageSource: "qrc:///qmlimages/VehicleUpsideDown.png"
- }
- VehicleRotationCal {
- visible: controller.orientationCalNoseDownSideVisible
- calValid: controller.orientationCalNoseDownSideDone
- calInProgress: controller.orientationCalNoseDownSideInProgress
- calInProgressText: controller.orientationCalNoseDownSideRotate ? "Rotate" : "Hold Still"
- imageSource: controller.orientationCalNoseDownSideRotate ? "qrc:///qmlimages/VehicleNoseDownRotate.png" : "qrc:///qmlimages/VehicleNoseDown.png"
- }
- VehicleRotationCal {
- visible: controller.orientationCalTailDownSideVisible
- calValid: controller.orientationCalTailDownSideDone
- calInProgress: controller.orientationCalTailDownSideInProgress
- calInProgressText: controller.orientationCalTailDownSideRotate ? "Rotate" : "Hold Still"
- imageSource: "qrc:///qmlimages/VehicleTailDown.png"
- }
- VehicleRotationCal {
- visible: controller.orientationCalLeftSideVisible
- calValid: controller.orientationCalLeftSideDone
- calInProgress: controller.orientationCalLeftSideInProgress
- calInProgressText: controller.orientationCalLeftSideRotate ? "Rotate" : "Hold Still"
- imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qmlimages/VehicleLeftRotate.png" : "qrc:///qmlimages/VehicleLeft.png"
- }
- VehicleRotationCal {
- visible: controller.orientationCalRightSideVisible
- calValid: controller.orientationCalRightSideDone
- calInProgress: controller.orientationCalRightSideInProgress
- calInProgressText: controller.orientationCalRightSideRotate ? "Rotate" : "Hold Still"
- imageSource: "qrc:///qmlimages/VehicleRight.png"
- }
- }
+ QGCLabel {
+ id: orientationCalAreaHelpText
+ anchors.margins: ScreenTools.defaultFontPixelWidth
+ anchors.top: orientationCalArea.top
+ anchors.left: orientationCalArea.left
+ width: parent.width
+ wrapMode: Text.WordWrap
+ font.pixelSize: ScreenTools.mediumFontPixelSize
}
- Column {
- anchors.leftMargin: ScreenTools.defaultFontPixelWidth
- anchors.left: orientationCalArea.right
- x: parent.width - rotationColumnWidth
+ Flow {
+ anchors.topMargin: ScreenTools.defaultFontPixelWidth
+ anchors.top: orientationCalAreaHelpText.bottom
+ anchors.left: orientationCalAreaHelpText.left
+ width: parent.width
+ height: parent.height - orientationCalAreaHelpText.implicitHeight
spacing: ScreenTools.defaultFontPixelWidth
- Column {
- spacing: ScreenTools.defaultFontPixelWidth
-
- QGCLabel {
- font.pixelSize: sideBarH1PointSize
- text: "Autopilot Orientation"
- }
-
- QGCLabel {
- width: parent.width
- wrapMode: Text.WordWrap
- text: boardRotationText
- }
-
- FactComboBox {
- id: boardRotationCombo
- width: rotationColumnWidth;
- model: rotations
- fact: sens_board_rot
- }
+ VehicleRotationCal {
+ visible: controller.orientationCalDownSideVisible
+ calValid: controller.orientationCalDownSideDone
+ calInProgress: controller.orientationCalDownSideInProgress
+ calInProgressText: controller.orientationCalDownSideRotate ? "Rotate" : "Hold Still"
+ imageSource: controller.orientationCalDownSideRotate ? "qrc:///qmlimages/VehicleDownRotate.png" : "qrc:///qmlimages/VehicleDown.png"
}
-
- Column {
- spacing: ScreenTools.defaultFontPixelWidth
-
- // Compass 0 rotation
- Component {
- id: compass0ComponentLabel2
-
- QGCLabel {
- font.pixelSize: sideBarH1PointSize
- text: "External Compass Orientation"
- }
- }
-
- Component {
- id: compass0ComponentCombo2
-
- FactComboBox {
- id: compass0RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag0_rot
- }
- }
-
- Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel2 : null }
- Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo2 : null }
+ VehicleRotationCal {
+ visible: controller.orientationCalUpsideDownSideVisible
+ calValid: controller.orientationCalUpsideDownSideDone
+ calInProgress: controller.orientationCalUpsideDownSideInProgress
+ calInProgressText: controller.orientationCalUpsideDownSideRotate ? "Rotate" : "Hold Still"
+ imageSource: "qrc:///qmlimages/VehicleUpsideDown.png"
}
-
- Column {
- spacing: ScreenTools.defaultFontPixelWidth
-
- // Compass 1 rotation
- Component {
- id: compass1ComponentLabel2
-
- QGCLabel {
- font.pixelSize: sideBarH1PointSize
- text: "External Compass 1 Orientation"
- }
- }
-
- Component {
- id: compass1ComponentCombo2
-
- FactComboBox {
- id: compass1RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag1_rot
- }
- }
-
- Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel2 : null }
- Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo2 : null }
+ VehicleRotationCal {
+ visible: controller.orientationCalNoseDownSideVisible
+ calValid: controller.orientationCalNoseDownSideDone
+ calInProgress: controller.orientationCalNoseDownSideInProgress
+ calInProgressText: controller.orientationCalNoseDownSideRotate ? "Rotate" : "Hold Still"
+ imageSource: controller.orientationCalNoseDownSideRotate ? "qrc:///qmlimages/VehicleNoseDownRotate.png" : "qrc:///qmlimages/VehicleNoseDown.png"
}
-
- Column {
- spacing: ScreenTools.defaultFontPixelWidth
-
- // Compass 2 rotation
- Component {
- id: compass2ComponentLabel2
-
- QGCLabel {
- font.pixelSize: sidebarH1PointSize
- text: "Compass 2 Orientation"
- }
- }
-
- Component {
- id: compass2ComponentCombo2
-
- FactComboBox {
- id: compass1RotationCombo
- width: rotationColumnWidth
- model: rotations
- fact: cal_mag2_rot
- }
- }
- Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel2 : null }
- Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo2 : null }
+ VehicleRotationCal {
+ visible: controller.orientationCalTailDownSideVisible
+ calValid: controller.orientationCalTailDownSideDone
+ calInProgress: controller.orientationCalTailDownSideInProgress
+ calInProgressText: controller.orientationCalTailDownSideRotate ? "Rotate" : "Hold Still"
+ imageSource: "qrc:///qmlimages/VehicleTailDown.png"
+ }
+ VehicleRotationCal {
+ visible: controller.orientationCalLeftSideVisible
+ calValid: controller.orientationCalLeftSideDone
+ calInProgress: controller.orientationCalLeftSideInProgress
+ calInProgressText: controller.orientationCalLeftSideRotate ? "Rotate" : "Hold Still"
+ imageSource: controller.orientationCalLeftSideRotate ? "qrc:///qmlimages/VehicleLeftRotate.png" : "qrc:///qmlimages/VehicleLeft.png"
+ }
+ VehicleRotationCal {
+ visible: controller.orientationCalRightSideVisible
+ calValid: controller.orientationCalRightSideDone
+ calInProgress: controller.orientationCalRightSideInProgress
+ calInProgressText: controller.orientationCalRightSideRotate ? "Rotate" : "Hold Still"
+ imageSource: "qrc:///qmlimages/VehicleRight.png"
}
}
}
+ } // Item - Cal display area
+
+ Loader {
+ id: rotationsLoader
+ anchors.topMargin: ScreenTools.defaultFontPixelHeight
+ anchors.right: parent.right
+ width: rotationColumnWidth
+ sourceComponent: rotationCombosComponent
+
+ property bool showCompassRotations: true
}
} // QGCViewPanel
} // QGCView
diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
index 8e54af9..30c3f04 100644
--- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
+++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
@@ -1,5 +1,5 @@
/*=====================================================================
-
+
QGroundControl Open Source Ground Control Station
(c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
@@ -40,7 +40,6 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
_nextButton(NULL),
_cancelButton(NULL),
_showOrientationCalArea(false),
- _gyroCalInProgress(false),
_magCalInProgress(false),
_accelCalInProgress(false),
_orientationCalDownSideDone(false),
@@ -69,6 +68,9 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
_orientationCalTailDownSideRotate(false),
_waitingForCancel(false)
{
+ _compassCal.setVehicle(_vehicle);
+ connect(&_compassCal, &APMCompassCal::vehicleTextMessage, this, &APMSensorsComponentController::_handleUASTextMessage);
+
APMAutoPilotPlugin * apmPlugin = qobject_cast<APMAutoPilotPlugin*>(_vehicle->autopilotPlugin());
_sensorsComponent = apmPlugin->sensorsComponent();
@@ -96,7 +98,9 @@ void APMSensorsComponentController::_startLogCalibration(void)
_compassButton->setEnabled(false);
_accelButton->setEnabled(false);
- _nextButton->setEnabled(true);
+ if (_accelCalInProgress) {
+ _nextButton->setEnabled(true);
+ }
_cancelButton->setEnabled(false);
}
@@ -145,7 +149,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
_accelButton->setEnabled(true);
_nextButton->setEnabled(false);
_cancelButton->setEnabled(false);
-
+
if (code == StopCalibrationSuccess) {
_resetInternalState();
_progressBar->setProperty("value", 1);
@@ -159,40 +163,37 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
_refreshParams();
switch (code) {
- case StopCalibrationSuccess:
- _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
- emit resetStatusTextArea();
- if (_magCalInProgress) {
- emit setCompassRotations();
- }
- break;
-
- case StopCalibrationCancelled:
- emit resetStatusTextArea();
- _hideAllCalAreas();
- break;
-
- default:
- // Assume failed
- _hideAllCalAreas();
- qgcApp()->showMessage("Calibration failed. Calibration log will be displayed.");
- break;
+ case StopCalibrationSuccess:
+ _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
+ emit resetStatusTextArea();
+ break;
+
+ case StopCalibrationCancelled:
+ emit resetStatusTextArea();
+ _hideAllCalAreas();
+ break;
+
+ default:
+ // Assume failed
+ _hideAllCalAreas();
+ qgcApp()->showMessage("Calibration failed. Calibration log will be displayed.");
+ break;
}
_magCalInProgress = false;
_accelCalInProgress = false;
- _gyroCalInProgress = false;
}
void APMSensorsComponentController::calibrateCompass(void)
{
_startLogCalibration();
- _uas->startCalibration(UASInterface::StartCalibrationMag);
+ _compassCal.startCalibration();
}
void APMSensorsComponentController::calibrateAccel(void)
{
_startLogCalibration();
+ _accelCalInProgress = true;
_uas->startCalibration(UASInterface::StartCalibrationAccel);
}
@@ -211,9 +212,21 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return;
}
+ if (text.contains("progress <")) {
+ QString percent = text.split("<").last().split(">").first();
+ bool ok;
+ int p = percent.toInt(&ok);
+ if (ok) {
+ Q_ASSERT(_progressBar);
+ _progressBar->setProperty("value", (float)(p / 100.0));
+ }
+ return;
+ }
+
QString anyKey("and press any");
if (text.contains(anyKey)) {
text = text.left(text.indexOf(anyKey)) + "and click Next to continue.";
+ _nextButton->setEnabled(true);
}
_appendStatusLog(text);
@@ -229,7 +242,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return;
}
-/*
// All calibration messages start with [cal]
QString calPrefix("[cal] ");
if (!text.startsWith(calPrefix)) {
@@ -243,7 +255,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_startVisualCalibration();
- text = parts[1];
if (text == "accel" || text == "mag" || text == "gyro") {
// Reset all progress indication
_orientationCalDownSideDone = false;
@@ -285,9 +296,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
_orientationCalRightSideVisible = true;
_orientationCalTailDownSideVisible = true;
_orientationCalNoseDownSideVisible = true;
- } else if (text == "gyro") {
- _gyroCalInProgress = true;
- _orientationCalDownSideVisible = true;
} else {
Q_ASSERT(false);
}
@@ -381,12 +389,21 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
return;
}
+ QString calCompletePrefix("calibration done:");
+ if (text.startsWith(calCompletePrefix)) {
+ _stopCalibration(StopCalibrationSuccess);
+ return;
+ }
+
if (text.startsWith("calibration cancelled")) {
_stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
return;
}
- */
+ if (text.startsWith("calibration failed")) {
+ _stopCalibration(StopCalibrationFailed);
+ return;
+ }
}
void APMSensorsComponentController::_refreshParams(void)
@@ -407,17 +424,17 @@ void APMSensorsComponentController::_refreshParams(void)
bool APMSensorsComponentController::fixedWing(void)
{
switch (_vehicle->vehicleType()) {
- case MAV_TYPE_FIXED_WING:
- case MAV_TYPE_VTOL_DUOROTOR:
- case MAV_TYPE_VTOL_QUADROTOR:
- case MAV_TYPE_VTOL_TILTROTOR:
- case MAV_TYPE_VTOL_RESERVED2:
- case MAV_TYPE_VTOL_RESERVED3:
- case MAV_TYPE_VTOL_RESERVED4:
- case MAV_TYPE_VTOL_RESERVED5:
- return true;
- default:
- return false;
+ case MAV_TYPE_FIXED_WING:
+ case MAV_TYPE_VTOL_DUOROTOR:
+ case MAV_TYPE_VTOL_QUADROTOR:
+ case MAV_TYPE_VTOL_TILTROTOR:
+ case MAV_TYPE_VTOL_RESERVED2:
+ case MAV_TYPE_VTOL_RESERVED3:
+ case MAV_TYPE_VTOL_RESERVED4:
+ case MAV_TYPE_VTOL_RESERVED5:
+ return true;
+ default:
+ return false;
}
}
@@ -434,12 +451,17 @@ void APMSensorsComponentController::_hideAllCalAreas(void)
void APMSensorsComponentController::cancelCalibration(void)
{
- // The firmware doesn't allow us to cancel calibration. The best we can do is wait
- // for it to timeout.
_waitingForCancel = true;
emit waitingForCancelChanged();
_cancelButton->setEnabled(false);
- _uas->stopCalibration();
+
+ if (_magCalInProgress) {
+ _compassCal.cancelCalibration();
+ } else {
+ // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
+ // for it to timeout.
+ _uas->stopCalibration();
+ }
}
void APMSensorsComponentController::nextClicked(void)
diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h
index 5df6839..b5a702b 100644
--- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h
+++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h
@@ -30,6 +30,7 @@
#include "FactPanelController.h"
#include "QGCLoggingCategory.h"
#include "APMSensorsComponent.h"
+#include "APMCompassCal.h"
Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog)
@@ -106,7 +107,6 @@ signals:
void orientationCalSidesRotateChanged(void);
void resetStatusTextArea(void);
void waitingForCancelChanged(void);
- void setCompassRotations(void);
void setupNeededChanged(void);
private slots:
@@ -129,6 +129,7 @@ private:
void _updateAndEmitShowOrientationCalArea(bool show);
+ APMCompassCal _compassCal;
APMSensorsComponent* _sensorsComponent;
QQuickItem* _statusLog;
@@ -139,10 +140,8 @@ private:
QQuickItem* _cancelButton;
QQuickItem* _orientationCalAreaHelpText;
- bool _showGyroCalArea;
bool _showOrientationCalArea;
- bool _gyroCalInProgress;
bool _magCalInProgress;
bool _accelCalInProgress;
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index dc367c7..6edc829 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -223,6 +223,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
_handleRCChannelsRaw(message);
break;
+ case MAVLINK_MSG_ID_RAW_IMU:
+ emit mavlinkRawImu(message);
+ break;
+ case MAVLINK_MSG_ID_SCALED_IMU:
+ emit mavlinkScaledImu1(message);
+ break;
+ case MAVLINK_MSG_ID_SCALED_IMU2:
+ emit mavlinkScaledImu2(message);
+ break;
+ case MAVLINK_MSG_ID_SCALED_IMU3:
+ emit mavlinkScaledImu3(message);
+ break;
}
emit mavlinkMessageReceived(message);
@@ -1035,7 +1047,7 @@ bool Vehicle::missingParameters(void)
return _autopilotPlugin->missingParameters();
}
-void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
+void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
{
mavlink_message_t msg;
mavlink_request_data_stream_t dataStream;
@@ -1048,8 +1060,12 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream);
- // We use sendMessageMultiple since we really want these to make it to the vehicle
- sendMessageMultiple(msg);
+ if (sendMultiple) {
+ // We use sendMessageMultiple since we really want these to make it to the vehicle
+ sendMessageMultiple(msg);
+ } else {
+ sendMessage(msg);
+ }
}
void Vehicle::_sendMessageMultipleNext(void)
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index f6b74b0..12633e9 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -204,7 +204,8 @@ public:
/// Requests the specified data stream from the vehicle
/// @param stream Stream which is being requested
/// @param rate Rate at which to send stream in Hz
- void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate);
+ /// @param sendMultiple Send multiple time to guarantee Vehicle reception
+ void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple = true);
bool missingParameters(void);
@@ -321,6 +322,11 @@ signals:
/// Remote control RSSI changed (0% - 100%)
void remoteControlRSSIChanged(uint8_t rssi);
+ void mavlinkRawImu(mavlink_message_t message);
+ void mavlinkScaledImu1(mavlink_message_t message);
+ void mavlinkScaledImu2(mavlink_message_t message);
+ void mavlinkScaledImu3(mavlink_message_t message);
+
private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
void _linkInactiveOrDeleted(LinkInterface* link);