diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c
index ab9d24df6..d6b52d2fa 100644
--- a/flight/Modules/Attitude/attitude.c
+++ b/flight/Modules/Attitude/attitude.c
@@ -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;
+ }
}
/**
* @}
diff --git a/ground/openpilotgcs/src/plugins/config/ccattitude.ui b/ground/openpilotgcs/src/plugins/config/ccattitude.ui
index 42bd28b5e..59afdeaf8 100644
--- a/ground/openpilotgcs/src/plugins/config/ccattitude.ui
+++ b/ground/openpilotgcs/src/plugins/config/ccattitude.ui
@@ -239,8 +239,6 @@
- verticalLayoutWidget
- groupBox_2
-
@@ -258,6 +256,20 @@
+ -
+
+
+ Get Current
+
+
+
+ -
+
+
+ Apply
+
+
+
-
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
index a63266998..3267fec5f 100644
--- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
@@ -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(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(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(getObjectManager()->getObject(QString("AttitudeSettings")));
saveObjectToSD(obj);
}
diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
index 14d34dbde..888fd9754 100644
--- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h
@@ -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;
diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml
index 48dd637e6..db067b7e6 100644
--- a/shared/uavobjectdefinition/attitudesettings.xml
+++ b/shared/uavobjectdefinition/attitudesettings.xml
@@ -2,7 +2,7 @@