|
|
@ -37,23 +37,25 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo |
|
|
|
_statusLog(NULL), |
|
|
|
_statusLog(NULL), |
|
|
|
_progressBar(NULL), |
|
|
|
_progressBar(NULL), |
|
|
|
_showGyroCalArea(false), |
|
|
|
_showGyroCalArea(false), |
|
|
|
_showAccelCalArea(false), |
|
|
|
_showOrientationCalArea(false), |
|
|
|
_showCompass0(false), |
|
|
|
_showCompass0(false), |
|
|
|
_showCompass1(false), |
|
|
|
_showCompass1(false), |
|
|
|
_showCompass2(false), |
|
|
|
_showCompass2(false), |
|
|
|
_gyroCalInProgress(false), |
|
|
|
_gyroCalInProgress(false), |
|
|
|
_accelCalDownSideDone(false), |
|
|
|
_magCalInProgress(false), |
|
|
|
_accelCalUpsideDownSideDone(false), |
|
|
|
_accelCalInProgress(false), |
|
|
|
_accelCalLeftSideDone(false), |
|
|
|
_orientationCalDownSideDone(false), |
|
|
|
_accelCalRightSideDone(false), |
|
|
|
_orientationCalUpsideDownSideDone(false), |
|
|
|
_accelCalNoseDownSideDone(false), |
|
|
|
_orientationCalLeftSideDone(false), |
|
|
|
_accelCalTailDownSideDone(false), |
|
|
|
_orientationCalRightSideDone(false), |
|
|
|
_accelCalDownSideInProgress(false), |
|
|
|
_orientationCalNoseDownSideDone(false), |
|
|
|
_accelCalUpsideDownSideInProgress(false), |
|
|
|
_orientationCalTailDownSideDone(false), |
|
|
|
_accelCalLeftSideInProgress(false), |
|
|
|
_orientationCalDownSideInProgress(false), |
|
|
|
_accelCalRightSideInProgress(false), |
|
|
|
_orientationCalUpsideDownSideInProgress(false), |
|
|
|
_accelCalNoseDownSideInProgress(false), |
|
|
|
_orientationCalLeftSideInProgress(false), |
|
|
|
_accelCalTailDownSideInProgress(false), |
|
|
|
_orientationCalRightSideInProgress(false), |
|
|
|
|
|
|
|
_orientationCalNoseDownSideInProgress(false), |
|
|
|
|
|
|
|
_orientationCalTailDownSideInProgress(false), |
|
|
|
_textLoggingStarted(false), |
|
|
|
_textLoggingStarted(false), |
|
|
|
_autopilot(autopilot) |
|
|
|
_autopilot(autopilot) |
|
|
|
{ |
|
|
|
{ |
|
|
@ -95,6 +97,9 @@ void SensorsComponentController::_startCalibration(void) |
|
|
|
|
|
|
|
|
|
|
|
void SensorsComponentController::_stopCalibration(bool failed) |
|
|
|
void SensorsComponentController::_stopCalibration(bool failed) |
|
|
|
{ |
|
|
|
{ |
|
|
|
|
|
|
|
_magCalInProgress = false; |
|
|
|
|
|
|
|
_accelCalInProgress = false; |
|
|
|
|
|
|
|
|
|
|
|
_compassButton->setEnabled(true); |
|
|
|
_compassButton->setEnabled(true); |
|
|
|
_gyroButton->setEnabled(true); |
|
|
|
_gyroButton->setEnabled(true); |
|
|
|
_accelButton->setEnabled(true); |
|
|
|
_accelButton->setEnabled(true); |
|
|
@ -160,6 +165,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in |
|
|
|
{ |
|
|
|
{ |
|
|
|
QString startingSidePrefix("Hold still, starting to measure "); |
|
|
|
QString startingSidePrefix("Hold still, starting to measure "); |
|
|
|
QString sideDoneSuffix(" side done, rotate to a different side"); |
|
|
|
QString sideDoneSuffix(" side done, rotate to a different side"); |
|
|
|
|
|
|
|
QString orientationDetectedSuffix(" orientation detected"); |
|
|
|
|
|
|
|
|
|
|
|
Q_UNUSED(compId); |
|
|
|
Q_UNUSED(compId); |
|
|
|
Q_UNUSED(severity); |
|
|
|
Q_UNUSED(severity); |
|
|
@ -194,83 +200,107 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in |
|
|
|
if (text == "gyro calibration: started") { |
|
|
|
if (text == "gyro calibration: started") { |
|
|
|
_updateAndEmitShowGyroCalArea(true); |
|
|
|
_updateAndEmitShowGyroCalArea(true); |
|
|
|
_updateAndEmitGyroCalInProgress(true); |
|
|
|
_updateAndEmitGyroCalInProgress(true); |
|
|
|
} else if (text == "accel calibration: started") { |
|
|
|
} else if (text == "accel calibration: started" || text == "mag calibration: started") { |
|
|
|
_accelCalDownSideDone = false; |
|
|
|
if (text == "accel calibration: started") { |
|
|
|
_accelCalUpsideDownSideDone = false; |
|
|
|
_accelCalInProgress = true; |
|
|
|
_accelCalLeftSideDone = false; |
|
|
|
_updateAndEmitCalInProgressText("Hold Still"); |
|
|
|
_accelCalRightSideDone = false; |
|
|
|
} else { |
|
|
|
_accelCalTailDownSideDone = false; |
|
|
|
_updateAndEmitCalInProgressText("Rotate"); |
|
|
|
_accelCalNoseDownSideDone = false; |
|
|
|
_magCalInProgress = true; |
|
|
|
_accelCalDownSideInProgress = false; |
|
|
|
} |
|
|
|
_accelCalUpsideDownSideInProgress = false; |
|
|
|
_orientationCalDownSideDone = false; |
|
|
|
_accelCalLeftSideInProgress = false; |
|
|
|
_orientationCalUpsideDownSideDone = false; |
|
|
|
_accelCalRightSideInProgress = false; |
|
|
|
_orientationCalLeftSideDone = false; |
|
|
|
_accelCalNoseDownSideInProgress = false; |
|
|
|
_orientationCalRightSideDone = false; |
|
|
|
_accelCalTailDownSideInProgress = false; |
|
|
|
_orientationCalTailDownSideDone = false; |
|
|
|
emit accelCalSidesDoneChanged(); |
|
|
|
_orientationCalNoseDownSideDone = false; |
|
|
|
emit accelCalSidesInProgressChanged(); |
|
|
|
_orientationCalDownSideInProgress = false; |
|
|
|
_updateAndEmitShowAccelCalArea(true); |
|
|
|
_orientationCalUpsideDownSideInProgress = false; |
|
|
|
|
|
|
|
_orientationCalLeftSideInProgress = false; |
|
|
|
|
|
|
|
_orientationCalRightSideInProgress = false; |
|
|
|
|
|
|
|
_orientationCalNoseDownSideInProgress = false; |
|
|
|
|
|
|
|
_orientationCalTailDownSideInProgress = false; |
|
|
|
|
|
|
|
emit orientationCalSidesDoneChanged(); |
|
|
|
|
|
|
|
emit orientationCalSidesInProgressChanged(); |
|
|
|
|
|
|
|
_updateAndEmitShowOrientationCalArea(true); |
|
|
|
} else if (text.startsWith(startingSidePrefix)) { |
|
|
|
} else if (text.startsWith(startingSidePrefix)) { |
|
|
|
QString side = text.right(text.length() - startingSidePrefix.length()).section(" ", 0, 0); |
|
|
|
QString side = text.right(text.length() - startingSidePrefix.length()).section(" ", 0, 0); |
|
|
|
qDebug() << "Side started" << side; |
|
|
|
qDebug() << "Side started" << side; |
|
|
|
if (side == "down") { |
|
|
|
if (side == "down") { |
|
|
|
_accelCalDownSideInProgress = true; |
|
|
|
_orientationCalDownSideInProgress = true; |
|
|
|
|
|
|
|
} else if (side == "up") { |
|
|
|
|
|
|
|
_orientationCalUpsideDownSideInProgress = true; |
|
|
|
|
|
|
|
} else if (side == "left") { |
|
|
|
|
|
|
|
_orientationCalLeftSideInProgress = true; |
|
|
|
|
|
|
|
} else if (side == "right") { |
|
|
|
|
|
|
|
_orientationCalRightSideInProgress = true; |
|
|
|
|
|
|
|
} else if (side == "front") { |
|
|
|
|
|
|
|
_orientationCalNoseDownSideInProgress = true; |
|
|
|
|
|
|
|
} else if (side == "back") { |
|
|
|
|
|
|
|
_orientationCalTailDownSideInProgress = true; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
emit orientationCalSidesInProgressChanged(); |
|
|
|
|
|
|
|
} else if (text.endsWith(orientationDetectedSuffix)) { |
|
|
|
|
|
|
|
QString side = text.section(" ", 0, 0); |
|
|
|
|
|
|
|
qDebug() << "Side started" << side; |
|
|
|
|
|
|
|
if (side == "down") { |
|
|
|
|
|
|
|
_orientationCalDownSideInProgress = true; |
|
|
|
} else if (side == "up") { |
|
|
|
} else if (side == "up") { |
|
|
|
_accelCalUpsideDownSideInProgress = true; |
|
|
|
_orientationCalUpsideDownSideInProgress = true; |
|
|
|
} else if (side == "left") { |
|
|
|
} else if (side == "left") { |
|
|
|
_accelCalLeftSideInProgress = true; |
|
|
|
_orientationCalLeftSideInProgress = true; |
|
|
|
} else if (side == "right") { |
|
|
|
} else if (side == "right") { |
|
|
|
_accelCalRightSideInProgress = true; |
|
|
|
_orientationCalRightSideInProgress = true; |
|
|
|
} else if (side == "front") { |
|
|
|
} else if (side == "front") { |
|
|
|
_accelCalNoseDownSideInProgress = true; |
|
|
|
_orientationCalNoseDownSideInProgress = true; |
|
|
|
} else if (side == "back") { |
|
|
|
} else if (side == "back") { |
|
|
|
_accelCalTailDownSideInProgress = true; |
|
|
|
_orientationCalTailDownSideInProgress = true; |
|
|
|
} |
|
|
|
} |
|
|
|
emit accelCalSidesInProgressChanged(); |
|
|
|
emit orientationCalSidesInProgressChanged(); |
|
|
|
} else if (text.endsWith(sideDoneSuffix)) { |
|
|
|
} else if (text.endsWith(sideDoneSuffix)) { |
|
|
|
QString side = text.section(" ", 0, 0); |
|
|
|
QString side = text.section(" ", 0, 0); |
|
|
|
qDebug() << "Side finished" << side; |
|
|
|
qDebug() << "Side finished" << side; |
|
|
|
if (side == "down") { |
|
|
|
if (side == "down") { |
|
|
|
_accelCalDownSideInProgress = false; |
|
|
|
_orientationCalDownSideInProgress = false; |
|
|
|
_accelCalDownSideDone = true; |
|
|
|
_orientationCalDownSideDone = true; |
|
|
|
} else if (side == "up") { |
|
|
|
} else if (side == "up") { |
|
|
|
_accelCalUpsideDownSideInProgress = false; |
|
|
|
_orientationCalUpsideDownSideInProgress = false; |
|
|
|
_accelCalUpsideDownSideDone = true; |
|
|
|
_orientationCalUpsideDownSideDone = true; |
|
|
|
} else if (side == "left") { |
|
|
|
} else if (side == "left") { |
|
|
|
_accelCalLeftSideInProgress = false; |
|
|
|
_orientationCalLeftSideInProgress = false; |
|
|
|
_accelCalLeftSideDone = true; |
|
|
|
_orientationCalLeftSideDone = true; |
|
|
|
} else if (side == "right") { |
|
|
|
} else if (side == "right") { |
|
|
|
_accelCalRightSideInProgress = false; |
|
|
|
_orientationCalRightSideInProgress = false; |
|
|
|
_accelCalRightSideDone = true; |
|
|
|
_orientationCalRightSideDone = true; |
|
|
|
} else if (side == "front") { |
|
|
|
} else if (side == "front") { |
|
|
|
_accelCalNoseDownSideInProgress = false; |
|
|
|
_orientationCalNoseDownSideInProgress = false; |
|
|
|
_accelCalNoseDownSideDone = true; |
|
|
|
_orientationCalNoseDownSideDone = true; |
|
|
|
} else if (side == "back") { |
|
|
|
} else if (side == "back") { |
|
|
|
_accelCalTailDownSideInProgress = false; |
|
|
|
_orientationCalTailDownSideInProgress = false; |
|
|
|
_accelCalTailDownSideDone = true; |
|
|
|
_orientationCalTailDownSideDone = true; |
|
|
|
} |
|
|
|
} |
|
|
|
emit accelCalSidesInProgressChanged(); |
|
|
|
emit orientationCalSidesInProgressChanged(); |
|
|
|
emit accelCalSidesDoneChanged(); |
|
|
|
emit orientationCalSidesDoneChanged(); |
|
|
|
} else if (text == "accel calibration: done") { |
|
|
|
} else if (text == "accel calibration: done" || text == "mag calibration: done") { |
|
|
|
_progressBar->setProperty("value", 1); |
|
|
|
_progressBar->setProperty("value", 1); |
|
|
|
_accelCalDownSideDone = true; |
|
|
|
_orientationCalDownSideDone = true; |
|
|
|
_accelCalUpsideDownSideDone = true; |
|
|
|
_orientationCalUpsideDownSideDone = true; |
|
|
|
_accelCalLeftSideDone = true; |
|
|
|
_orientationCalLeftSideDone = true; |
|
|
|
_accelCalRightSideDone = true; |
|
|
|
_orientationCalRightSideDone = true; |
|
|
|
_accelCalTailDownSideDone = true; |
|
|
|
_orientationCalTailDownSideDone = true; |
|
|
|
_accelCalNoseDownSideDone = true; |
|
|
|
_orientationCalNoseDownSideDone = true; |
|
|
|
_accelCalDownSideInProgress = false; |
|
|
|
_orientationCalDownSideInProgress = false; |
|
|
|
_accelCalUpsideDownSideInProgress = false; |
|
|
|
_orientationCalUpsideDownSideInProgress = false; |
|
|
|
_accelCalLeftSideInProgress = false; |
|
|
|
_orientationCalLeftSideInProgress = false; |
|
|
|
_accelCalRightSideInProgress = false; |
|
|
|
_orientationCalRightSideInProgress = false; |
|
|
|
_accelCalNoseDownSideInProgress = false; |
|
|
|
_orientationCalNoseDownSideInProgress = false; |
|
|
|
_accelCalTailDownSideInProgress = false; |
|
|
|
_orientationCalTailDownSideInProgress = false; |
|
|
|
emit accelCalSidesDoneChanged(); |
|
|
|
emit orientationCalSidesDoneChanged(); |
|
|
|
emit accelCalSidesInProgressChanged(); |
|
|
|
emit orientationCalSidesInProgressChanged(); |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
} else if (text == "gyro calibration: done") { |
|
|
|
} else if (text == "gyro calibration: done") { |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
} else if (text == "mag calibration: done" || text == "dpress calibration: done") { |
|
|
|
} else if (text == "dpress calibration: done") { |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
_stopCalibration(false /* success */); |
|
|
|
} else if (text.endsWith(" calibration: failed")) { |
|
|
|
} else if (text.endsWith(" calibration: failed")) { |
|
|
|
_stopCalibration(true /* failed */); |
|
|
|
_stopCalibration(true /* failed */); |
|
|
@ -321,14 +351,20 @@ void SensorsComponentController::_updateAndEmitShowGyroCalArea(bool show) |
|
|
|
emit showGyroCalAreaChanged(); |
|
|
|
emit showGyroCalAreaChanged(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void SensorsComponentController::_updateAndEmitShowAccelCalArea(bool show) |
|
|
|
void SensorsComponentController::_updateAndEmitShowOrientationCalArea(bool show) |
|
|
|
{ |
|
|
|
{ |
|
|
|
_showAccelCalArea = show; |
|
|
|
_showOrientationCalArea = show; |
|
|
|
emit showAccelCalAreaChanged(); |
|
|
|
emit showOrientationCalAreaChanged(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void SensorsComponentController::_hideAllCalAreas(void) |
|
|
|
void SensorsComponentController::_hideAllCalAreas(void) |
|
|
|
{ |
|
|
|
{ |
|
|
|
_updateAndEmitShowGyroCalArea(false); |
|
|
|
_updateAndEmitShowGyroCalArea(false); |
|
|
|
_updateAndEmitShowAccelCalArea(false); |
|
|
|
_updateAndEmitShowOrientationCalArea(false); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void SensorsComponentController::_updateAndEmitCalInProgressText(const QString& text) |
|
|
|
|
|
|
|
{ |
|
|
|
|
|
|
|
_calInProgressText = text; |
|
|
|
|
|
|
|
emit calInProgressTextChanged(text); |
|
|
|
} |
|
|
|
} |
|
|
|