|
|
|
@ -8,6 +8,9 @@
@@ -8,6 +8,9 @@
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#include <QTimer> |
|
|
|
|
#include <QDir> |
|
|
|
|
#include <QXmlStreamReader> |
|
|
|
|
#include <QMessageBox> |
|
|
|
|
|
|
|
|
|
#include "QGCVehicleConfig.h" |
|
|
|
|
#include "UASManager.h" |
|
|
|
@ -109,20 +112,33 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
@@ -109,20 +112,33 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
|
|
|
|
|
|
|
|
|
|
void QGCVehicleConfig::setTrimPositions() |
|
|
|
|
{ |
|
|
|
|
// Set trim for roll, pitch, yaw, throttle
|
|
|
|
|
rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll
|
|
|
|
|
rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch
|
|
|
|
|
rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw
|
|
|
|
|
// Set trim to min if stick is close to min
|
|
|
|
|
if (abs(rcValue[rcMapping[3]] - rcMin[rcMapping[3]]) < 100) |
|
|
|
|
{ |
|
|
|
|
rcTrim[rcMapping[3]] = rcMin[rcMapping[3]]; // throttle
|
|
|
|
|
} |
|
|
|
|
// Set trim to max if stick is close to max
|
|
|
|
|
if (abs(rcValue[rcMapping[3]] - rcMax[rcMapping[3]]) < 100) |
|
|
|
|
else if (abs(rcValue[rcMapping[3]] - rcMax[rcMapping[3]]) < 100) |
|
|
|
|
{ |
|
|
|
|
rcTrim[rcMapping[3]] = rcMax[rcMapping[3]]; // throttle
|
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
// Reject
|
|
|
|
|
QMessageBox msgBox; |
|
|
|
|
msgBox.setText(tr("Throttle Stick Trim Position Invalid")); |
|
|
|
|
msgBox.setInformativeText(tr("The throttle stick is not in the min position. Please set it to the minimum value")); |
|
|
|
|
msgBox.setStandardButtons(QMessageBox::Ok); |
|
|
|
|
msgBox.setDefaultButton(QMessageBox::Ok); |
|
|
|
|
(void)msgBox.exec(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Set trim for roll, pitch, yaw, throttle
|
|
|
|
|
rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll
|
|
|
|
|
rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch
|
|
|
|
|
rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw
|
|
|
|
|
|
|
|
|
|
rcTrim[rcMapping[4]] = ((rcMax[rcMapping[4]] - rcMin[rcMapping[4]]) / 2.0f) + rcMin[rcMapping[4]]; // mode sw
|
|
|
|
|
rcTrim[rcMapping[5]] = ((rcMax[rcMapping[5]] - rcMin[rcMapping[5]]) / 2.0f) + rcMin[rcMapping[5]]; // aux 1
|
|
|
|
|
rcTrim[rcMapping[6]] = ((rcMax[rcMapping[6]] - rcMin[rcMapping[6]]) / 2.0f) + rcMin[rcMapping[6]]; // aux 2
|
|
|
|
|