Commit a329074c authored by Don Gagne's avatar Don Gagne

APM Stack Compass Calibration

parent 9eb2f360
......@@ -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 \
......
This diff is collapsed.
/*=====================================================================
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
/*=====================================================================
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)
......
......@@ -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;
......
......@@ -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)
......
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment