From a329074cc35ec6e618aab0d620ac33b6ef097a08 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 23 Dec 2015 21:29:02 -0800 Subject: [PATCH] APM Stack Compass Calibration --- qgroundcontrol.pro | 2 + src/AutoPilotPlugins/APM/APMCompassCal.cc | 737 ++++++++++++++++++ src/AutoPilotPlugins/APM/APMCompassCal.h | 180 +++++ .../APM/APMSensorsComponent.qml | 644 ++++++--------- .../APM/APMSensorsComponentController.cc | 110 +-- .../APM/APMSensorsComponentController.h | 5 +- src/Vehicle/Vehicle.cc | 22 +- src/Vehicle/Vehicle.h | 8 +- 8 files changed, 1266 insertions(+), 442 deletions(-) create mode 100644 src/AutoPilotPlugins/APM/APMCompassCal.cc create mode 100644 src/AutoPilotPlugins/APM/APMCompassCal.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 167a06a67..15c1ba3cb 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 000000000..f5072c0c6 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMCompassCal.cc @@ -0,0 +1,737 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009, 2015 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +#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(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); + worker_data.z[cur_mag] = reinterpret_cast(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_magautopilotPlugin(); + if (result == calibrate_return_ok) { + for (unsigned cur_mag=0; cur_maggetParameterFact(-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_magcalibration_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 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 000000000..8eb014f46 --- /dev/null +++ b/src/AutoPilotPlugins/APM/APMCompassCal.h @@ -0,0 +1,180 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009, 2015 QGROUNDCONTROL PROJECT + + 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 . + + ======================================================================*/ + +#ifndef APMCompassCal_H +#define APMCompassCal_H + +#include +#include +#include + +#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 f5674dee7..e68c03399 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 8e54af9e9..30c3f0433 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 @@ -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(_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 5df683928..b5a702b01 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 dc367c760..6edc82999 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 f6b74b0e4..12633e93b 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); -- 2.22.0