1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

LP-590 Store and clear BoardLevelTrim while calibrating accelerometers

This commit is contained in:
Laurent Lalanne 2018-04-04 03:14:45 +02:00
parent 9fcebfe2c9
commit de6b29c6a1
2 changed files with 21 additions and 12 deletions

View File

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

View File

@ -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;