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

Update the AttitudeSetting object to make the board rotation more human

Browse Source 
readable and update the GCS fields appropriately.
This commit is contained in:
James Cotton 2011-05-05 13:04:56 -05:00
parent bff970d414
commit 7e418866be
5 changed files with 50 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -307,7 +307,6 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
float rotationQuat[4];
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
@ -319,37 +318,22 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
// Indicates not to expend cycles on rotation
if(attitudeSettings.RotationQuaternion[0] == 126) {
rotate = 0;
rotationQuat[0] = 1;
rotationQuat[1] = rotationQuat[2] = rotationQuat[3] = 0;
return;
}
rotate = 1;
rotationQuat[0] = (float) attitudeSettings.RotationQuaternion[0] / 127.0;
rotationQuat[1] = (float) attitudeSettings.RotationQuaternion[1] / 127.0f;
rotationQuat[2] = (float) attitudeSettings.RotationQuaternion[2] / 127.0f;
rotationQuat[3] = (float) attitudeSettings.RotationQuaternion[3] / 127.0f;
float len = sqrt(rotationQuat[0] * rotationQuat[0] + rotationQuat[1] * rotationQuat[1] +
rotationQuat[2] * rotationQuat[2] + rotationQuat[3] * rotationQuat[3]);
// Sanitize input. Shouldn't do anything functional.
if(len < 1e-3) {
// Really wrong value
rotationQuat[0] = 1;
rotationQuat[1] = rotationQuat[2] = rotationQuat[3] = 0;
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
rotate = 0;
// Shouldn't be used but to be safe
float rotationQuat[4] = {1,0,0,0};
Quaternion2R(rotationQuat, R);
} else {
rotationQuat[0] /= len;
rotationQuat[1] /= len;
rotationQuat[2] /= len;
rotationQuat[3] /= len;
}
Quaternion2R(rotationQuat, R);
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
}
/**
* @}

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

@ -239,8 +239,6 @@
</item>
</layout>
</widget>
<zorder>verticalLayoutWidget</zorder>
<zorder>groupBox_2</zorder>
</widget>
</item>
<item>
@ -258,6 +256,20 @@
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="getCurrentButton">
<property name="text">
<string>Get Current</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="applyButton">
<property name="text">
<string>Apply</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="saveButton">
<property name="toolTip">

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

@ -37,10 +37,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)));
connect(ui->saveButton,SIGNAL(clicked()),this,SLOT(saveAttitudeSettings()));
connect(ui->applyButton,SIGNAL(clicked()),this,SLOT(applyAttitudeSettings()));
connect(ui->getCurrentButton,SIGNAL(clicked()),this,SLOT(getCurrentAttitudeSettings()));
}
ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget()
@ -97,25 +96,25 @@ void ConfigCCAttitudeWidget::timeout() {
}
void ConfigCCAttitudeWidget::attitudeBiasChanged(int val) {
void ConfigCCAttitudeWidget::applyAttitudeSettings() {
UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
UAVObjectField * field = settings->getField("RotationQuaternion");
UAVObjectField * field = settings->getField("BoardRotation");
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);
field->setValue(ui->rollBias->value(),0);
field->setValue(ui->pitchBias->value(),1);
field->setValue(ui->yawBias->value(),2);
settings->updated();
}
void ConfigCCAttitudeWidget::getCurrentAttitudeSettings() {
UAVDataObject * settings = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
UAVObjectField * field = settings->getField("BoardRotation");
ui->rollBias->setValue(field->getDouble(0));
ui->pitchBias->setValue(field->getDouble(1));
ui->yawBias->setValue(field->getDouble(2));
}
void ConfigCCAttitudeWidget::startAccelCalibration() {
QMutexLocker locker(&startStop);
@ -144,6 +143,8 @@ void ConfigCCAttitudeWidget::startAccelCalibration() {
}
void ConfigCCAttitudeWidget::saveAttitudeSettings() {
applyAttitudeSettings();
UAVDataObject * obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeSettings")));
saveObjectToSD(obj);
}

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

@ -47,11 +47,12 @@ public:
~ConfigCCAttitudeWidget();
private slots:
void attitudeRawUpdated(UAVObject *);
void attitudeRawUpdated(UAVObject * obj);
void timeout();
void startAccelCalibration();
void attitudeBiasChanged(int val);
void saveAttitudeSettings();
void applyAttitudeSettings();
void getCurrentAttitudeSettings();
private:
QMutex startStop;

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -2,7 +2,7 @@
<object name="AttitudeSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="AccelBias" units="lsb" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="RotationQuaternion" units="quaternion * 255" type="int8" elementnames="q1,q2,q3,q4" defaultvalue="127,0,0,0"/>
<field name="BoardRotation" units="deg" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.01"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>