mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-350: Resolve this bug by explicit local stack alignment annotations. The Win32 ABI only guarantees an 8-byte aligned stack, but SSE requires 16-byte alignment.
OP-191: Include code for computing the misalignment rotation, but #ifdef it out until the feature freeze is lifted. OP-191: Clean up UI to inform the user that HomeLocation must be set prior to multi-point cal, and disable multi-point cal's start button until it is done. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3082 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
3fc88f69e2
commit
9b8494e9f6
20
ground/openpilotgcs/src/libs/eigen/Eigen/align-function.h
Normal file
20
ground/openpilotgcs/src/libs/eigen/Eigen/align-function.h
Normal file
@ -0,0 +1,20 @@
|
||||
#ifndef OP_EIGEN_ALIGN_FUNCTION_H
|
||||
#define OP_EIGEN_ALIGN_FUNCTION_H
|
||||
|
||||
/*
|
||||
* The purpose of this macro is to force the alignment of the stack to a
|
||||
* 16-byte boundary on systems that don't provide it automatically. At
|
||||
* this time, only GCC on Win32 is know to require it. MSVC on Win32 may
|
||||
* as well.
|
||||
*/
|
||||
|
||||
#ifdef __GNUC__
|
||||
# ifdef WIN32
|
||||
# define FORCE_ALIGN_FUNC __attribute__((force_align_arg_pointer))
|
||||
# else
|
||||
# define FORCE_ALIGN_FUNC
|
||||
# endif
|
||||
#error Unknown compiler. You may need to provide a definition of FORCE_ALIGN_FUNC
|
||||
#endif
|
||||
|
||||
#endif // !defined OP_EIGEN_ALIGN_FUNCTION_H
|
@ -403,14 +403,14 @@ TODO: is this necessary? Measurement could be auto updated every second or so, o
|
||||
<html><head><meta name="qrichtext" content="1" /><style type="text/css">
|
||||
p, li { white-space: pre-wrap; }
|
||||
</style></head><body style=" font-family:'Sans'; font-size:10pt; font-weight:400; font-style:normal;">
|
||||
<table style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;">
|
||||
<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;">
|
||||
<tr>
|
||||
<td style="border: none;">
|
||||
<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Help</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Step #1 and #2 are really necessary. Steps #3 and #4 will help you achieve the best possible results.</span></p>
|
||||
<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-weight:600;"></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">#1: Six point calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute the scale for all sensors on the INS. Press &quot;Start&quot; to begin calibration, and follow the instructions which will be displayed here.</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">#1: Multi-Point calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute the scale for all sensors on the INS. Press &quot;Start&quot; to begin calibration, and follow the instructions which will be displayed here. Note that your HomeLocation must be set first, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p>
|
||||
<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">#2: Sensor noise calibration:</span></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute sensor variance under standard conditions. You can leave your engines running at low to mid throttle to take their vibration into account before pressing &quot;Start&quot;.</span></p>
|
||||
|
@ -36,6 +36,7 @@
|
||||
#include <QtGui/QPushButton>
|
||||
#include <QThread>
|
||||
#include <iostream>
|
||||
#include <Eigen/align-function.h>
|
||||
|
||||
#include "calibration.h"
|
||||
|
||||
@ -206,6 +207,9 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
|
||||
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation")));
|
||||
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this , SLOT(enableHomeLocSave(UAVObject*)));
|
||||
|
||||
// Don't enable multi-point calibration until HomeLocation is set.
|
||||
m_ahrs->sixPointsStart->setEnabled(obj->getField("Set")->getValue().toBool());
|
||||
|
||||
// Connect the signals
|
||||
connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(launchAHRSCalibration()));
|
||||
connect(m_ahrs->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
|
||||
@ -564,6 +568,7 @@ void ConfigAHRSWidget::saveAHRSCalibration()
|
||||
|
||||
}
|
||||
|
||||
FORCE_ALIGN_FUNC
|
||||
void ConfigAHRSWidget::attitudeRawUpdated(UAVObject * obj)
|
||||
{
|
||||
QMutexLocker lock(&attitudeRawUpdateLock);
|
||||
@ -774,8 +779,18 @@ updateBias(UAVObjectField *scale,
|
||||
bias->setDouble(final_bias(1), 1);
|
||||
bias->setDouble(final_bias(2), 2);
|
||||
}
|
||||
|
||||
void
|
||||
updateRotation(UAVObjectField *rotation, const Vector3f& updateRotation)
|
||||
{
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
rotation->setDouble(updateRotation[i], i);
|
||||
}
|
||||
}
|
||||
|
||||
} // !namespace (anon)
|
||||
|
||||
FORCE_ALIGN_FUNC
|
||||
void ConfigAHRSWidget::computeScaleBias()
|
||||
{
|
||||
// Extract the local magnetic and gravitational field vectors from HomeLocation.
|
||||
@ -826,12 +841,12 @@ void ConfigAHRSWidget::computeScaleBias()
|
||||
std::cout << "gyro bias: " << gyroBias.transpose()
|
||||
<< "\ngyro's acceleration sensitivity:\n" << accelSensitivity << std::endl;
|
||||
|
||||
// Calibrate alignment between the accelerometer and gyro, taking the accelerometer as the
|
||||
// Calibrate alignment between the accelerometer and magnetometer, taking the mag as the
|
||||
// reference.
|
||||
Vector3f magRotation;
|
||||
calibration_misalignment(magRotation, accel_data, -Vector3f::UnitZ()*localGravity,
|
||||
Vector3f accelRotation;
|
||||
calibration_misalignment(accelRotation, accel_data, -Vector3f::UnitZ()*localGravity,
|
||||
mag_data, localMagField, n_positions);
|
||||
std::cout << "magnetometer rotation vector: " << magRotation.transpose() << std::endl;
|
||||
std::cout << "magnetometer rotation vector: " << accelRotation.transpose() << std::endl;
|
||||
|
||||
// Update the calibration scalars
|
||||
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
|
||||
@ -851,7 +866,11 @@ void ConfigAHRSWidget::computeScaleBias()
|
||||
obj->getField(QString("gyro_bias")),
|
||||
gyroBias);
|
||||
|
||||
// TODO: Misalignment matrix between accelerometer and magnetometer
|
||||
#if 0
|
||||
// TODO: Enable after v1.0 feature freeze is lifted.
|
||||
updateRotation(obj->getField(QString("accel_rotation")), accelRotation);
|
||||
#endif
|
||||
|
||||
obj->updated();
|
||||
|
||||
position = -1; //set to run again
|
||||
@ -870,26 +889,24 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
|
||||
UAVObjectField *field = obj->getField(QString("accel_scale"));
|
||||
// TODO: Figure out how to load these directly from the saved metadata
|
||||
// about default values
|
||||
field->setDouble(0.035, 0);
|
||||
field->setDouble(0.035, 1);
|
||||
field->setDouble(0.035, 2);
|
||||
field->setDouble(0.0359, 0);
|
||||
field->setDouble(0.0359, 1);
|
||||
field->setDouble(0.0359, 2);
|
||||
|
||||
field = obj->getField(QString("accel_bias"));
|
||||
field->setDouble(-72.5, 0);
|
||||
field->setDouble(-72.5, 1);
|
||||
field->setDouble(72.5, 2);
|
||||
field->setDouble(-73.5, 0);
|
||||
field->setDouble(-73.5, 1);
|
||||
field->setDouble(73.5, 2);
|
||||
|
||||
field = obj->getField(QString("accel_ortho"));
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
field->setDouble(0, i);
|
||||
}
|
||||
|
||||
#if 1
|
||||
field = obj->getField(QString("gyro_bias"));
|
||||
field->setDouble(28.5,0);
|
||||
field->setDouble(-28.5,1);
|
||||
field->setDouble(28.6,2);
|
||||
#endif
|
||||
field->setDouble(23,0);
|
||||
field->setDouble(-23,1);
|
||||
field->setDouble(23,2);
|
||||
|
||||
field = obj->getField(QString("mag_scale"));
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
@ -901,9 +918,16 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
|
||||
field->setDouble(0, i);
|
||||
}
|
||||
|
||||
#if 0
|
||||
// TODO: Enable after the feature freeze is lifted.
|
||||
field = obj->getField(QString("accel_rotation"));
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
field->setDouble(0, i);
|
||||
}
|
||||
#endif
|
||||
|
||||
obj->updated();
|
||||
|
||||
// TODO: What is this?
|
||||
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
|
||||
field = obj->getField(QString("BiasCorrectedRaw"));
|
||||
field->setValue("FALSE");
|
||||
@ -1020,6 +1044,7 @@ void ConfigAHRSWidget::enableHomeLocSave(UAVObject * obj)
|
||||
UAVObjectField *field = obj->getField(QString("Set"));
|
||||
if (field) {
|
||||
m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool());
|
||||
m_ahrs->sixPointsStart->setEnabled(obj->getField("Set")->getValue().toBool());
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
x
Reference in New Issue
Block a user