Skip to content

Commit 188446a

Browse files
HTRamseyHTRamsey
committed
APM: Fix Compass Callibration Progress Bar
1 parent 4bf6018 commit 188446a

File tree

2 files changed

+139
-126
lines changed

2 files changed

+139
-126
lines changed

‎src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

Lines changed: 138 additions & 126 deletions
Original file line numberDiff line numberDiff line change
@@ -384,7 +384,7 @@ void APMSensorsComponentController::_handleTextMessage(int sysid, int componenti
384384
void APMSensorsComponentController::_refreshParams()
385385
{
386386
static const QStringList fastRefreshList = {
387-
QStringLiteral("COMPASS_OFS_X"), QStringLiteral("COMPASS_OFS_X"), QStringLiteral("COMPASS_OFS_X"),
387+
QStringLiteral("COMPASS_OFS_X"), QStringLiteral("COMPASS_OFS_Y"), QStringLiteral("COMPASS_OFS_Z"),
388388
QStringLiteral("INS_ACCOFFS_X"), QStringLiteral("INS_ACCOFFS_Y"), QStringLiteral("INS_ACCOFFS_Z")
389389
};
390390

@@ -497,80 +497,154 @@ void APMSensorsComponentController::_handleCommandAck(const mavlink_message_t &m
497497

498498
void APMSensorsComponentController::_handleMagCalProgress(const mavlink_message_t &message)
499499
{
500-
if (_calTypeInProgress == QGCMAVLink::CalibrationMag) {
501-
mavlink_mag_cal_progress_t magCalProgress{};
502-
mavlink_msg_mag_cal_progress_decode(&message, &magCalProgress);
500+
if (_calTypeInProgress != QGCMAVLink::CalibrationMag) {
501+
return;
502+
}
503503

504-
qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalProgress id:mask:pct"
505-
<< magCalProgress.compass_id
506-
<< magCalProgress.cal_mask
507-
<< magCalProgress.completion_pct;
504+
mavlink_mag_cal_progress_t magCalProgress{};
505+
mavlink_msg_mag_cal_progress_decode(&message, &magCalProgress);
508506

509-
// How many compasses are we calibrating?
510-
int compassCalCount = 0;
511-
for (int i = 0; i < 3; i++) {
512-
if (magCalProgress.cal_mask & (1 << i)) {
513-
compassCalCount++;
514-
}
515-
}
507+
qCDebug(APMSensorsComponentControllerVerboseLog) << "id:mask:pct"
508+
<< magCalProgress.compass_id
509+
<< magCalProgress.cal_mask
510+
<< magCalProgress.completion_pct;
516511

517-
if ((magCalProgress.compass_id < 3) && (compassCalCount != 0)) {
518-
// Each compass gets a portion of the overall progress
519-
_rgCompassCalProgress[magCalProgress.compass_id] = magCalProgress.completion_pct / compassCalCount;
512+
// How many compasses are we calibrating?
513+
int compassCalCount = 0;
514+
for (int i = 0; i < 3; i++) {
515+
if (magCalProgress.cal_mask & (1 << i)) {
516+
compassCalCount++;
520517
}
518+
}
521519

522-
if (_progressBar) {
523-
(void) _progressBar->setProperty("value", static_cast<float>(_rgCompassCalProgress[0] + _rgCompassCalProgress[1] + (_rgCompassCalProgress[2]) / 100.0));
524-
}
520+
if ((magCalProgress.compass_id < 3) && (compassCalCount != 0)) {
521+
// Each compass gets a portion of the overall progress
522+
_rgCompassCalProgress[magCalProgress.compass_id] = magCalProgress.completion_pct / compassCalCount;
523+
}
524+
525+
if (_progressBar) {
526+
(void) _progressBar->setProperty("value", static_cast<float>(_rgCompassCalProgress[0] + _rgCompassCalProgress[1] + _rgCompassCalProgress[2]) / 100.0);
525527
}
526528
}
527529

528530
void APMSensorsComponentController::_handleMagCalReport(const mavlink_message_t &message)
529531
{
530-
if (_calTypeInProgress == QGCMAVLink::CalibrationMag) {
531-
mavlink_mag_cal_report_t magCalReport{};
532-
mavlink_msg_mag_cal_report_decode(&message, &magCalReport);
532+
if (_calTypeInProgress != QGCMAVLink::CalibrationMag) {
533+
return;
534+
}
533535

534-
qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalReport id:mask:status:fitness"
535-
<< magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness;
536+
mavlink_mag_cal_report_t magCalReport{};
537+
mavlink_msg_mag_cal_report_decode(&message, &magCalReport);
536538

537-
bool additionalCompassCompleted = false;
538-
if ((magCalReport.compass_id < 3) && !_rgCompassCalComplete[magCalReport.compass_id]) {
539-
if (magCalReport.cal_status == MAG_CAL_SUCCESS) {
540-
_appendStatusLog(tr("Compass %1 calibration complete").arg(magCalReport.compass_id));
541-
} else {
542-
_appendStatusLog(tr("Compass %1 calibration below quality threshold").arg(magCalReport.compass_id));
543-
}
544-
_rgCompassCalComplete[magCalReport.compass_id] = true;
545-
_rgCompassCalSucceeded[magCalReport.compass_id] = magCalReport.cal_status == MAG_CAL_SUCCESS;
546-
_rgCompassCalFitness[magCalReport.compass_id] = magCalReport.fitness;
547-
additionalCompassCompleted = true;
539+
qCDebug(APMSensorsComponentControllerVerboseLog) << "id:mask:status:fitness"
540+
<< magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness;
541+
542+
bool additionalCompassCompleted = false;
543+
if ((magCalReport.compass_id < 3) && !_rgCompassCalComplete[magCalReport.compass_id]) {
544+
if (magCalReport.cal_status == MAG_CAL_SUCCESS) {
545+
_appendStatusLog(tr("Compass %1 calibration complete").arg(magCalReport.compass_id));
546+
} else {
547+
_appendStatusLog(tr("Compass %1 calibration below quality threshold").arg(magCalReport.compass_id));
548548
}
549+
_rgCompassCalComplete[magCalReport.compass_id] = true;
550+
_rgCompassCalSucceeded[magCalReport.compass_id] = (magCalReport.cal_status == MAG_CAL_SUCCESS);
551+
_rgCompassCalFitness[magCalReport.compass_id] = magCalReport.fitness;
552+
additionalCompassCompleted = true;
553+
}
549554

550-
if (_rgCompassCalComplete[0] && _rgCompassCalComplete[1] &&_rgCompassCalComplete[2]) {
551-
for (int i = 0; i < 3; i++) {
552-
qCDebug(APMSensorsComponentControllerLog) << QString("Onboard compass call report #%1: succeed:fitness %2:%3").arg(i).arg(_rgCompassCalSucceeded[i]).arg(_rgCompassCalFitness[i]);
553-
}
554-
emit compass1CalFitnessChanged(_rgCompassCalFitness[0]);
555-
emit compass2CalFitnessChanged(_rgCompassCalFitness[1]);
556-
emit compass3CalFitnessChanged(_rgCompassCalFitness[2]);
557-
emit compass1CalSucceededChanged(_rgCompassCalSucceeded[0]);
558-
emit compass2CalSucceededChanged(_rgCompassCalSucceeded[1]);
559-
emit compass3CalSucceededChanged(_rgCompassCalSucceeded[2]);
560-
if (_rgCompassCalSucceeded[0] && _rgCompassCalSucceeded[1] && _rgCompassCalSucceeded[2]) {
561-
_appendStatusLog(tr("All compasses calibrated successfully"));
562-
_appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW FOR NEW SETTINGS TO TAKE AFFECT"));
563-
_stopCalibration(StopCalibrationSuccessShowLog);
564-
} else {
565-
_appendStatusLog(tr("Compass calibration failed"));
566-
_appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW AND RETRY COMPASS CALIBRATION PRIOR TO FLIGHT"));
567-
_stopCalibration(StopCalibrationFailed);
568-
}
569-
} else if (additionalCompassCompleted) {
570-
_appendStatusLog(tr("Continue rotating..."));
555+
if (_rgCompassCalComplete[0] && _rgCompassCalComplete[1] &&_rgCompassCalComplete[2]) {
556+
for (int i = 0; i < 3; i++) {
557+
qCDebug(APMSensorsComponentControllerLog) << QString("Onboard compass call report #%1: succeed:fitness %2:%3").arg(i).arg(_rgCompassCalSucceeded[i]).arg(_rgCompassCalFitness[i]);
571558
}
559+
emit compass1CalFitnessChanged(_rgCompassCalFitness[0]);
560+
emit compass2CalFitnessChanged(_rgCompassCalFitness[1]);
561+
emit compass3CalFitnessChanged(_rgCompassCalFitness[2]);
562+
emit compass1CalSucceededChanged(_rgCompassCalSucceeded[0]);
563+
emit compass2CalSucceededChanged(_rgCompassCalSucceeded[1]);
564+
emit compass3CalSucceededChanged(_rgCompassCalSucceeded[2]);
565+
if (_rgCompassCalSucceeded[0] && _rgCompassCalSucceeded[1] && _rgCompassCalSucceeded[2]) {
566+
_appendStatusLog(tr("All compasses calibrated successfully"));
567+
_appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW FOR NEW SETTINGS TO TAKE AFFECT"));
568+
_stopCalibration(StopCalibrationSuccessShowLog);
569+
} else {
570+
_appendStatusLog(tr("Compass calibration failed"));
571+
_appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW AND RETRY COMPASS CALIBRATION PRIOR TO FLIGHT"));
572+
_stopCalibration(StopCalibrationFailed);
573+
}
574+
} else if (additionalCompassCompleted) {
575+
_appendStatusLog(tr("Continue rotating..."));
576+
}
577+
}
578+
579+
bool APMSensorsComponentController::_handleCmdLongAccelcalVehiclePos(const mavlink_command_long_t &commandLong)
580+
{
581+
bool updateImages = false;
572582

583+
switch (static_cast<ACCELCAL_VEHICLE_POS>(static_cast<int>(commandLong.param1))) {
584+
case ACCELCAL_VEHICLE_POS_LEVEL:
585+
if (!_orientationCalDownSideInProgress) {
586+
updateImages = true;
587+
_orientationCalDownSideInProgress = true;
588+
_nextButton->setEnabled(true);
589+
}
590+
break;
591+
case ACCELCAL_VEHICLE_POS_LEFT:
592+
if (!_orientationCalLeftSideInProgress) {
593+
updateImages = true;
594+
_orientationCalDownSideDone = true;
595+
_orientationCalDownSideInProgress = false;
596+
_orientationCalLeftSideInProgress = true;
597+
(void) _progressBar->setProperty("value", static_cast<qreal>(17 / 100.0));
598+
}
599+
break;
600+
case ACCELCAL_VEHICLE_POS_RIGHT:
601+
if (!_orientationCalRightSideInProgress) {
602+
updateImages = true;
603+
_orientationCalLeftSideDone = true;
604+
_orientationCalLeftSideInProgress = false;
605+
_orientationCalRightSideInProgress = true;
606+
(void) _progressBar->setProperty("value", static_cast<qreal>(34 / 100.0));
607+
}
608+
break;
609+
case ACCELCAL_VEHICLE_POS_NOSEDOWN:
610+
if (!_orientationCalNoseDownSideInProgress) {
611+
updateImages = true;
612+
_orientationCalRightSideDone = true;
613+
_orientationCalRightSideInProgress = false;
614+
_orientationCalNoseDownSideInProgress = true;
615+
(void) _progressBar->setProperty("value", static_cast<qreal>(51 / 100.0));
616+
}
617+
break;
618+
case ACCELCAL_VEHICLE_POS_NOSEUP:
619+
if (!_orientationCalTailDownSideInProgress) {
620+
updateImages = true;
621+
_orientationCalNoseDownSideDone = true;
622+
_orientationCalNoseDownSideInProgress = false;
623+
_orientationCalTailDownSideInProgress = true;
624+
(void) _progressBar->setProperty("value", static_cast<qreal>(68 / 100.0));
625+
}
626+
break;
627+
case ACCELCAL_VEHICLE_POS_BACK:
628+
if (!_orientationCalUpsideDownSideInProgress) {
629+
updateImages = true;
630+
_orientationCalTailDownSideDone = true;
631+
_orientationCalTailDownSideInProgress = false;
632+
_orientationCalUpsideDownSideInProgress = true;
633+
(void) _progressBar->setProperty("value", static_cast<qreal>(85 / 100.0));
634+
}
635+
break;
636+
case ACCELCAL_VEHICLE_POS_SUCCESS:
637+
_stopCalibration(StopCalibrationSuccess);
638+
break;
639+
case ACCELCAL_VEHICLE_POS_FAILED:
640+
_stopCalibration(StopCalibrationFailed);
641+
break;
642+
case ACCELCAL_VEHICLE_POS_ENUM_END:
643+
default:
644+
break;
573645
}
646+
647+
return updateImages;
574648
}
575649

576650
void APMSensorsComponentController::_handleCommandLong(const mavlink_message_t &message)
@@ -581,75 +655,13 @@ void APMSensorsComponentController::_handleCommandLong(const mavlink_message_t &
581655
mavlink_msg_command_long_decode(&message, &commandLong);
582656

583657
if (commandLong.command == MAV_CMD_ACCELCAL_VEHICLE_POS) {
584-
switch (static_cast<ACCELCAL_VEHICLE_POS>(static_cast<int>(commandLong.param1))) {
585-
case ACCELCAL_VEHICLE_POS_LEVEL:
586-
if (!_orientationCalDownSideInProgress) {
587-
updateImages = true;
588-
_orientationCalDownSideInProgress = true;
589-
_nextButton->setEnabled(true);
590-
}
591-
break;
592-
case ACCELCAL_VEHICLE_POS_LEFT:
593-
if (!_orientationCalLeftSideInProgress) {
594-
updateImages = true;
595-
_orientationCalDownSideDone = true;
596-
_orientationCalDownSideInProgress = false;
597-
_orientationCalLeftSideInProgress = true;
598-
(void) _progressBar->setProperty("value", static_cast<qreal>(17 / 100.0));
599-
}
600-
break;
601-
case ACCELCAL_VEHICLE_POS_RIGHT:
602-
if (!_orientationCalRightSideInProgress) {
603-
updateImages = true;
604-
_orientationCalLeftSideDone = true;
605-
_orientationCalLeftSideInProgress = false;
606-
_orientationCalRightSideInProgress = true;
607-
(void) _progressBar->setProperty("value", static_cast<qreal>(34 / 100.0));
608-
}
609-
break;
610-
case ACCELCAL_VEHICLE_POS_NOSEDOWN:
611-
if (!_orientationCalNoseDownSideInProgress) {
612-
updateImages = true;
613-
_orientationCalRightSideDone = true;
614-
_orientationCalRightSideInProgress = false;
615-
_orientationCalNoseDownSideInProgress = true;
616-
(void) _progressBar->setProperty("value", static_cast<qreal>(51 / 100.0));
617-
}
618-
break;
619-
case ACCELCAL_VEHICLE_POS_NOSEUP:
620-
if (!_orientationCalTailDownSideInProgress) {
621-
updateImages = true;
622-
_orientationCalNoseDownSideDone = true;
623-
_orientationCalNoseDownSideInProgress = false;
624-
_orientationCalTailDownSideInProgress = true;
625-
(void) _progressBar->setProperty("value", static_cast<qreal>(68 / 100.0));
626-
}
627-
break;
628-
case ACCELCAL_VEHICLE_POS_BACK:
629-
if (!_orientationCalUpsideDownSideInProgress) {
630-
updateImages = true;
631-
_orientationCalTailDownSideDone = true;
632-
_orientationCalTailDownSideInProgress = false;
633-
_orientationCalUpsideDownSideInProgress = true;
634-
(void) _progressBar->setProperty("value", static_cast<qreal>(85 / 100.0));
635-
}
636-
break;
637-
case ACCELCAL_VEHICLE_POS_SUCCESS:
638-
_stopCalibration(StopCalibrationSuccess);
639-
break;
640-
case ACCELCAL_VEHICLE_POS_FAILED:
641-
_stopCalibration(StopCalibrationFailed);
642-
break;
643-
case ACCELCAL_VEHICLE_POS_ENUM_END:
644-
default:
645-
break;
646-
}
658+
updateImages = _handleCmdLongAccelcalVehiclePos(commandLong);
659+
}
647660

648-
if (updateImages) {
649-
emit orientationCalSidesDoneChanged();
650-
emit orientationCalSidesInProgressChanged();
651-
emit orientationCalSidesRotateChanged();
652-
}
661+
if (updateImages) {
662+
emit orientationCalSidesDoneChanged();
663+
emit orientationCalSidesInProgressChanged();
664+
emit orientationCalSidesRotateChanged();
653665
}
654666
}
655667

‎src/AutoPilotPlugins/APM/APMSensorsComponentController.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -137,6 +137,7 @@ private slots:
137137
void _handleCommandAck(const mavlink_message_t &message);
138138
void _handleMagCalProgress(const mavlink_message_t &message);
139139
void _handleMagCalReport(const mavlink_message_t &message);
140+
bool _handleCmdLongAccelcalVehiclePos(const mavlink_command_long_t &commandLong);
140141
void _handleCommandLong(const mavlink_message_t &message);
141142
void _restorePreviousCompassCalFitness();
142143

0 commit comments

Comments
 (0)