1
0
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:
jonathan 2011-03-27 20:26:29 +00:00 committed by jonathan
parent 3fc88f69e2
commit 9b8494e9f6
3 changed files with 65 additions and 20 deletions

View 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

View File

@ -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 "Start" 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 "Start" 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 "Start".</span></p>

View File

@ -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());
}
}