rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

CC-24: Rotate the CC board at any angle relative to "flat and level" with GCS

Browse Source 
config plugin updates.  Has not been tested in flight yet although seems
sensible so please be careful when using this code.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3166 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-04-15 06:37:16 +00:00 committed by peabody124
parent a2333c259f
commit 3afdc7e41c
8 changed files with 314 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/Libraries/CoordinateConversions.c
View File

@ -376,6 +376,10 @@ float VectorMagnitude(const float v[3])
return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}
/**
* @brief Compute the inverse of a quaternion
* @param [in][out] q The matrix to invert
*/
void quat_inverse(float q[4])
{
q[1] = -q[1];
@ -383,6 +387,11 @@ void quat_inverse(float q[4])
q[3] = -q[3];
}
/**
* @brief Duplicate a quaternion
* @param[in] q quaternion in
* @param[out] qnew quaternion to copy to
*/
void quat_copy(const float q[4], float qnew[4])
{
qnew[0] = q[0];
@ -391,6 +400,12 @@ void quat_copy(const float q[4], float qnew[4])
qnew[3] = q[3];
}
/**
* @brief Multiply two quaternions into a third
* @param[in] q1 First quaternion
* @param[in] q2 Second quaternion
* @param[out] qout Output quaternion
*/
void quat_mult(const float q1[4], const float q2[4], float qout[4])
{
qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
@ -398,3 +413,16 @@ void quat_mult(const float q1[4], const float q2[4], float qout[4])
qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}
/**
* @brief Rotate a vector by a rotation matrix
* @param[in] R a three by three rotation matrix (first index is row)
* @param[in] vec the source vector
* @param[out] vec_out the output vector
*/
void rot_mult(float R[3][3], const float vec[3], float vec_out[3])
{
vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2];
vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2];
vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2];
}

2
flight/Libraries/inc/CoordinateConversions.h
View File

@ -74,6 +74,6 @@ float VectorMagnitude(const float v[3]);
void quat_inverse(float q[4]);
void quat_copy(const float q[4], float qnew[4]);
void quat_mult(const float q1[4], const float q2[4], float qout[4]);
void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
#endif // COORDINATECONVERSIONS_H_

58
flight/Modules/Attitude/attitude.c
View File

@ -84,8 +84,8 @@ static float accelKp = 0;
static float yawBiasRate = 0;
static float gyroGain = 0.42;
static int16_t accelbias[3];
static float rotationQuat[4] = {1,0,0,0};
static float q[4] = {1,0,0,0};
static float R[3][3];
static int8_t rotate = 0;
/**
@ -175,16 +175,6 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
// Because most crafts wont get enough information from gravity to zero yaw gyro
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] *
yawBiasRate;
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
@ -199,16 +189,38 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
} while ( (i < 32) && (samples_remaining > 0) );
attitudeRaw->gyrotemp[0] = samples_remaining;
attitudeRaw->gyrotemp[1] = i;
x -= accelbias[0] * i;
y -= accelbias[1] * i;
z -= accelbias[2] * i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = ((float)x * 0.004f * 9.81f) / i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = ((float)y * 0.004f * 9.81f) / i;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = ((float)z * 0.004f * 9.81f) / i;
float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
if(rotate) {
// TODO: rotate sensors too so stabilization is well behaved
}
float vec_out[3];
rot_mult(R, accel, vec_out);
attitudeRaw->accels[0] = vec_out[0];
attitudeRaw->accels[1] = vec_out[1];
attitudeRaw->accels[2] = vec_out[2];
rot_mult(R, attitudeRaw->gyros, vec_out);
attitudeRaw->gyros[0] = vec_out[0];
attitudeRaw->gyros[1] = vec_out[1];
attitudeRaw->gyros[2] = vec_out[2];
} else {
attitudeRaw->accels[0] = accel[0];
attitudeRaw->accels[1] = accel[1];
attitudeRaw->accels[2] = accel[2];
}
// Scale accels and correct bias
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * 0.004f * 9.81f;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
// Because most crafts wont get enough information from gravity to zero yaw gyro
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
@ -283,12 +295,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
if(rotate) {
// Rotate the attitude q from the state q to allow board to be mounted at weird angles
quat_mult(q,rotationQuat,&attitudeActual.q1);
} else {
quat_copy(q, &attitudeActual.q1);
}
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
@ -300,6 +307,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
float rotationQuat[4];
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
@ -340,6 +349,7 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
rotationQuat[3] /= len;
}
Quaternion2R(rotationQuat, R);
}
/**
* @}

311
ground/openpilotgcs/src/plugins/config/ccattitude.ui
View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>400</width>
<height>300</height>
<height>307</height>
</rect>
</property>
<property name="windowTitle">
@ -15,93 +15,232 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
<widget class="QGroupBox" name="groupBox_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
<property name="minimumSize">
<size>
<width>0</width>
<height>110</height>
</size>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_2">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>CopterControl Attitude Calibration</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label">
<property name="text">
<string>Place aircraft flat before computing</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="zeroBias">
<property name="toolTip">
<string>Launch horizontal calibration.</string>
</property>
<property name="text">
<string>Zero Accel Bias</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="zeroBiasProgress">
<property name="value">
<number>0</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="status">
<property name="text">
<string>Click &quot;Zero Accel Bias&quot; to start</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
<property name="title">
<string>Attitude Rotation</string>
</property>
<widget class="QSpinBox" name="rollBias">
<property name="geometry">
<rect>
<x>10</x>
<y>70</y>
<width>81</width>
<height>25</height>
</rect>
</property>
<property name="minimum">
<number>-180</number>
</property>
<property name="maximum">
<number>180</number>
</property>
</widget>
<widget class="QSpinBox" name="pitchBias">
<property name="geometry">
<rect>
<x>120</x>
<y>70</y>
<width>81</width>
<height>25</height>
</rect>
</property>
<property name="minimum">
<number>-90</number>
</property>
<property name="maximum">
<number>90</number>
</property>
</widget>
<widget class="QSpinBox" name="yawBias">
<property name="geometry">
<rect>
<x>220</x>
<y>70</y>
<width>81</width>
<height>25</height>
</rect>
</property>
<property name="minimum">
<number>-180</number>
</property>
<property name="maximum">
<number>180</number>
</property>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>50</y>
<width>91</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Roll</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
<widget class="QLabel" name="label_3">
<property name="geometry">
<rect>
<x>120</x>
<y>50</y>
<width>81</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Pitch</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
<widget class="QLabel" name="label_4">
<property name="geometry">
<rect>
<x>220</x>
<y>50</y>
<width>81</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Yaw</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
<widget class="QLabel" name="label_5">
<property name="geometry">
<rect>
<x>10</x>
<y>30</y>
<width>341</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Select the amount to rotate the attitude</string>
</property>
</widget>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>140</height>
</size>
</property>
<property name="title">
<string>Attitude Calibration</string>
</property>
<widget class="QWidget" name="verticalLayoutWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>20</y>
<width>371</width>
<height>111</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="margin">
<number>3</number>
</property>
<item>
<widget class="QLabel" name="label">
<property name="text">
<string>Place aircraft flat before computing</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="status">
<property name="text">
<string>Click &quot;Zero Accel Bias&quot; to start</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="zeroBias">
<property name="toolTip">
<string>Launch horizontal calibration.</string>
</property>
<property name="text">
<string>Zero Accel Bias</string>
</property>
</widget>
</item>
<item>
<widget class="QProgressBar" name="zeroBiasProgress">
<property name="value">
<number>0</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
<zorder>verticalLayoutWidget</zorder>
<zorder>groupBox_2</zorder>
</widget>
</item>
<item>

1
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -3,6 +3,7 @@ TARGET = Config
QT += svg
include(../../openpilotgcsplugin.pri)
include(../../libs/utils/utils.pri)
include(../../plugins/uavtalk/uavtalk.pri)
include(../../plugins/coreplugin/coreplugin.pri)
include(../../plugins/uavobjects/uavobjects.pri)

23
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -26,6 +26,7 @@
*/
#include "configccattitudewidget.h"
#include "ui_ccattitude.h"
#include "utils/coordinateconversions.h"
#include <QMutexLocker>
#include <QErrorMessage>
#include <QDebug>
@ -37,6 +38,9 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
ui->setupUi(this);
connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration()));
connect(ui->saveButton,SIGNAL(clicked()),this,SLOT(saveAttitudeSettings()));
connect(ui->rollBias,SIGNAL(valueChanged(int)),this,SLOT(attitudeBiasChanged(int)));
connect(ui->pitchBias,SIGNAL(valueChanged(int)),this,SLOT(attitudeBiasChanged(int)));
connect(ui->yawBias,SIGNAL(valueChanged(int)),this,SLOT(attitudeBiasChanged(int)));
}
ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget()
@ -90,6 +94,25 @@ void ConfigCCAttitudeWidget::timeout() {
errmsg.exec();
}
void ConfigCCAttitudeWidget::attitudeBiasChanged(int val) {
UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
UAVObjectField * field = settings->getField("RotationQuaternion");
float RPY[3], q[4];
RPY[0] = ui->rollBias->value();
RPY[1] = ui->pitchBias->value();
RPY[2] = ui->yawBias->value();
Utils::CoordinateConversions().RPY2Quaternion(RPY,q);
field->setDouble(q[0]*127,0);
field->setDouble(q[1]*127,1);
field->setDouble(q[2]*127,2);
field->setDouble(q[3]*127,3);
settings->updated();
}
void ConfigCCAttitudeWidget::startAccelCalibration() {
QMutexLocker locker(&startStop);

1
ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
View File

@ -50,6 +50,7 @@ private slots:
void attitudeRawUpdated(UAVObject *);
void timeout();
void startAccelCalibration();
void attitudeBiasChanged(int val);
void saveAttitudeSettings();
private:

2
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -17,7 +17,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
<number>2</number>
<number>1</number>
</property>
<widget class="QWidget" name="tab">
<attribute name="title">