From 9fcebfe2c9192943b85b04c54f8dfc3d94d51d46 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 4 Apr 2018 02:01:57 +0200 Subject: [PATCH 1/3] LP-590 Zero BoardLevelTrim values while doing GyroBias calibration --- .../config/calibration/gyrobiascalibrationmodel.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp b/ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp index b65b30055..3953aaacc 100644 --- a/ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp +++ b/ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp @@ -69,9 +69,13 @@ void GyroBiasCalibrationModel::start() // Disable gyro bias correction while calibrating attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; // Zero board rotation - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + // Zero board level trim + attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] = 0; + attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0; + attitudeSettings->setData(attitudeSettingsData); UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata(); From de6b29c6a1c998a5215885a6d7a33c9cd9238b67 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 4 Apr 2018 03:14:45 +0200 Subject: [PATCH 2/3] LP-590 Store and clear BoardLevelTrim while calibrating accelerometers --- .../src/plugins/config/configrevowidget.cpp | 32 ++++++++++++------- .../gcs/src/plugins/config/configrevowidget.h | 1 + 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp index 2fb72481e..0a7d3ffe6 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.cpp +++ b/ground/gcs/src/plugins/config/configrevowidget.cpp @@ -238,19 +238,25 @@ void ConfigRevoWidget::storeAndClearBoardRotation() if (!isBoardRotationStored) { UAVObjectUpdaterHelper updateHelper; - // Store current board rotation + // Store current board rotation and board level trim isBoardRotationStored = true; AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); AttitudeSettings::DataFields data = attitudeSettings->getData(); - storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; - storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; + storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; + storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] = data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]; + storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH]; - // Set board rotation to no rotation - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; - data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + // Set board rotation to zero + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0; + data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0; + + // Set board level trim to zero + data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] = 0; + data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0; attitudeSettings->setData(data, false); updateHelper.doObjectAndWait(attitudeSettings); @@ -281,13 +287,15 @@ void ConfigRevoWidget::recallBoardRotation() // Recall current board rotation isBoardRotationStored = false; - // Restore the flight controller board rotation + // Restore the flight controller board rotation and board level trim AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); Q_ASSERT(attitudeSettings); AttitudeSettings::DataFields data = attitudeSettings->getData(); - data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; - data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; - data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; + data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]; + data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]; + data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH]; + data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL] = storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]; + data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH]; attitudeSettings->setData(data, false); updateHelper.doObjectAndWait(attitudeSettings); diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h index 896cabd60..c7d1a14e0 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.h +++ b/ground/gcs/src/plugins/config/configrevowidget.h @@ -63,6 +63,7 @@ private: // Board rotation store/recall for FC and for aux mag qint16 storedBoardRotation[3]; + float storedBoardLevelTrim[2]; qint16 auxMagStoredBoardRotation[3]; bool isBoardRotationStored; From 54b308047943e482cafd2eb249847a5dce7470e0 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 10 Apr 2018 18:14:39 +0200 Subject: [PATCH 3/3] LP-590 Store BoardRotation as float value --- ground/gcs/src/plugins/config/configrevowidget.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h index c7d1a14e0..e76c25027 100644 --- a/ground/gcs/src/plugins/config/configrevowidget.h +++ b/ground/gcs/src/plugins/config/configrevowidget.h @@ -62,7 +62,7 @@ private: Ui_RevoSensorsWidget *m_ui; // Board rotation store/recall for FC and for aux mag - qint16 storedBoardRotation[3]; + float storedBoardRotation[3]; float storedBoardLevelTrim[2]; qint16 auxMagStoredBoardRotation[3]; bool isBoardRotationStored;