zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Improved and validated RC calibration

QGC4.4
Lorenz Meier 13 years ago
parent
fb7663d37e
commit
b3e21a8b19
2 changed files with 78 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 136
      src/ui/QGCVehicleConfig.cc
  2. 10
      src/ui/QGCVehicleConfig.h

136
src/ui/QGCVehicleConfig.cc
Unescape View File

@ -66,6 +66,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) : @@ -66,6 +66,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
for (unsigned int i = 0; i < chanMax; i++)
{
rcValue[i] = UINT16_MAX;
rcMapping[i] = i;
}
updateTimer.setInterval(150);
@ -102,14 +103,14 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled) @@ -102,14 +103,14 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled)
void QGCVehicleConfig::setTrimPositions()
{
// Set trim for roll, pitch, yaw, throttle
rcTrim[rcMapping[0]] = rcValue[0]; // roll
rcTrim[rcMapping[1]] = rcValue[1]; // pitch
rcTrim[rcMapping[2]] = rcValue[2]; // yaw
rcTrim[rcMapping[3]] = rcMin[3]; // throttle
rcTrim[rcMapping[4]] = ((rcMax[4] - rcMin[4]) / 2.0f) + rcMin[4]; // mode sw
rcTrim[rcMapping[5]] = ((rcMax[5] - rcMin[5]) / 2.0f) + rcMin[5]; // aux 1
rcTrim[rcMapping[5]] = ((rcMax[6] - rcMin[6]) / 2.0f) + rcMin[6]; // aux 2
rcTrim[rcMapping[5]] = ((rcMax[7] - rcMin[7]) / 2.0f) + rcMin[7]; // aux 3
rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll
rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch
rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw
rcTrim[rcMapping[3]] = rcMin[rcMapping[3]]; // throttle
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
rcTrim[rcMapping[7]] = ((rcMax[rcMapping[7]] - rcMin[rcMapping[7]]) / 2.0f) + rcMin[rcMapping[7]]; // aux 3
}
void QGCVehicleConfig::detectChannelInversion()
@ -249,7 +250,7 @@ void QGCVehicleConfig::writeCalibrationRC() @@ -249,7 +250,7 @@ void QGCVehicleConfig::writeCalibrationRC()
for (unsigned int i = 0; i < chanCount; ++i)
{
qDebug() << "SENDING" << minTpl.arg(i+1) << rcMin[i];
//qDebug() << "SENDING" << minTpl.arg(i+1) << rcMin[i];
mav->setParameter(0, minTpl.arg(i+1), rcMin[i]);
QGC::SLEEP::usleep(50000);
mav->setParameter(0, trimTpl.arg(i+1), rcTrim[i]);
@ -265,9 +266,9 @@ void QGCVehicleConfig::writeCalibrationRC() @@ -265,9 +266,9 @@ void QGCVehicleConfig::writeCalibrationRC()
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[2]+1));
mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[2]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[3]+1));
mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[3]+1));
QGC::SLEEP::usleep(50000);
mav->setParameter(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1));
QGC::SLEEP::usleep(50000);
@ -334,6 +335,9 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) @@ -334,6 +335,9 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
}
}
// Raw value
rcValue[chan] = val;
// Normalized value
float normalized;
@ -354,21 +358,18 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) @@ -354,21 +358,18 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
if (chan == rcMapping[0])
{
// ROLL
rcValue[0] = val;
rcRoll = normalized;
}
else if (chan == rcMapping[1])
if (chan == rcMapping[1])
{
// PITCH
rcValue[1] = val;
rcPitch = normalized;
}
else if (chan == rcMapping[2])
if (chan == rcMapping[2])
{
rcValue[2] = val;
rcYaw = normalized;
}
else if (chan == rcMapping[3])
if (chan == rcMapping[3])
{
// THROTTLE
if (rcRev[chan]) {
@ -377,32 +378,27 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) @@ -377,32 +378,27 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
rcThrottle = normalized;
}
rcValue[3] = val;
rcThrottle = qBound(0.0f, rcThrottle, 1.0f);
}
else if (chan == rcMapping[4])
if (chan == rcMapping[4])
{
// MODE SWITCH
rcMode = normalized;
rcValue[4] = val;
}
else if (chan == rcMapping[5])
if (chan == rcMapping[5])
{
// AUX1
rcAux1 = normalized;
rcValue[5] = val;
}
else if (chan == rcMapping[6])
if (chan == rcMapping[6])
{
// AUX2
rcAux2 = normalized;
rcValue[6] = val;
}
else if (chan == rcMapping[7])
if (chan == rcMapping[7])
{
// AUX3
rcAux3 = normalized;
rcValue[7] = val;
}
changed = true;
@ -410,6 +406,39 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) @@ -410,6 +406,39 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val)
//qDebug() << "RC CHAN:" << chan << "PPM:" << val << "NORMALIZED:" << normalized;
}
void QGCVehicleConfig::updateInvertedCheckboxes(int index)
{
unsigned int mapindex = rcMapping[index];
switch (mapindex)
{
case 0:
ui->invertCheckBox->setChecked(rcRev[index]);
break;
case 1:
ui->invertCheckBox_2->setChecked(rcRev[index]);
break;
case 2:
ui->invertCheckBox_3->setChecked(rcRev[index]);
break;
case 3:
ui->invertCheckBox_4->setChecked(rcRev[index]);
break;
case 4:
ui->invertCheckBox_5->setChecked(rcRev[index]);
break;
case 5:
ui->invertCheckBox_6->setChecked(rcRev[index]);
break;
case 6:
ui->invertCheckBox_7->setChecked(rcRev[index]);
break;
case 7:
ui->invertCheckBox_8->setChecked(rcRev[index]);
break;
}
}
void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
Q_UNUSED(uas);
@ -431,7 +460,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete @@ -431,7 +460,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (minTpl.exactMatch(parameterName)) {
bool ok;
unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
qDebug() << "PARAM:" << parameterName << "index:" << index;
//qDebug() << "PARAM:" << parameterName << "index:" << index;
if (ok && (index >= 0) && (index < chanMax))
{
rcMin[index] = value.toInt();
@ -462,36 +491,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete @@ -462,36 +491,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (ok && (index >= 0) && (index < chanMax))
{
rcRev[index] = (value.toInt() == -1) ? true : false;
unsigned int mapindex = rcMapping[index];
switch (mapindex)
{
case 0:
ui->invertCheckBox->setChecked(rcRev[index]);
break;
case 1:
ui->invertCheckBox_2->setChecked(rcRev[index]);
break;
case 2:
ui->invertCheckBox_3->setChecked(rcRev[index]);
break;
case 3:
ui->invertCheckBox_4->setChecked(rcRev[index]);
break;
case 4:
ui->invertCheckBox_5->setChecked(rcRev[index]);
break;
case 5:
ui->invertCheckBox_6->setChecked(rcRev[index]);
break;
case 6:
ui->invertCheckBox_7->setChecked(rcRev[index]);
break;
case 7:
ui->invertCheckBox_8->setChecked(rcRev[index]);
break;
}
updateInvertedCheckboxes(index);
}
}
@ -653,31 +653,31 @@ void QGCVehicleConfig::updateView() @@ -653,31 +653,31 @@ void QGCVehicleConfig::updateView()
ui->throttleSlider->setValue(rcThrottle * 100);
}
ui->chanLabel->setText(QString("%1/%2").arg(rcValue[0]).arg(rcRoll, 5, 'f', 2, QChar(' ')));
ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[1]).arg(rcPitch, 5, 'f', 2, QChar(' ')));
ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[2]).arg(rcYaw, 5, 'f', 2, QChar(' ')));
ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[3]).arg(rcThrottle, 5, 'f', 2, QChar(' ')));
ui->chanLabel->setText(QString("%1/%2").arg(rcValue[rcMapping[0]]).arg(rcRoll, 5, 'f', 2, QChar(' ')));
ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[rcMapping[1]]).arg(rcPitch, 5, 'f', 2, QChar(' ')));
ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[rcMapping[2]]).arg(rcYaw, 5, 'f', 2, QChar(' ')));
ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[rcMapping[3]]).arg(rcThrottle, 5, 'f', 2, QChar(' ')));
ui->modeSwitchSlider->setValue(rcMode * 50 + 50);
ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[4]).arg(rcMode, 5, 'f', 2, QChar(' ')));
ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[rcMapping[4]]).arg(rcMode, 5, 'f', 2, QChar(' ')));
if (rcValue[5] != UINT16_MAX) {
if (rcValue[rcMapping[5]] != UINT16_MAX) {
ui->aux1Slider->setValue(rcAux1 * 50 + 50);
ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[5]).arg(rcAux1, 5, 'f', 2, QChar(' ')));
ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[rcMapping[5]]).arg(rcAux1, 5, 'f', 2, QChar(' ')));
} else {
ui->chanLabel_6->setText(tr("---"));
}
if (rcValue[6] != UINT16_MAX) {
if (rcValue[rcMapping[6]] != UINT16_MAX) {
ui->aux2Slider->setValue(rcAux2 * 50 + 50);
ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[6]).arg(rcAux2, 5, 'f', 2, QChar(' ')));
ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[rcMapping[6]]).arg(rcAux2, 5, 'f', 2, QChar(' ')));
} else {
ui->chanLabel_7->setText(tr("---"));
}
if (rcValue[7] != UINT16_MAX) {
if (rcValue[rcMapping[7]] != UINT16_MAX) {
ui->aux3Slider->setValue(rcAux3 * 50 + 50);
ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[7]).arg(rcAux3, 5, 'f', 2, QChar(' ')));
ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[rcMapping[7]]).arg(rcAux3, 5, 'f', 2, QChar(' ')));
} else {
ui->chanLabel_8->setText(tr("---"));
}

10
src/ui/QGCVehicleConfig.h
Unescape View File

@ -49,34 +49,42 @@ public slots: @@ -49,34 +49,42 @@ public slots:
/** Set the RC channel */
void setRollChan(int channel) {
rcMapping[0] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setPitchChan(int channel) {
rcMapping[1] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setYawChan(int channel) {
rcMapping[2] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setThrottleChan(int channel) {
rcMapping[3] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setModeChan(int channel) {
rcMapping[4] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setAux1Chan(int channel) {
rcMapping[5] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setAux2Chan(int channel) {
rcMapping[6] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set the RC channel */
void setAux3Chan(int channel) {
rcMapping[7] = channel - 1;
updateInvertedCheckboxes(channel - 1);
}
/** Set channel inversion status */
@ -130,6 +138,8 @@ protected slots: @@ -130,6 +138,8 @@ protected slots:
void setRCType(int type);
/** Check timeouts */
void checktimeOuts();
/** Update checkbox status */
void updateInvertedCheckboxes(int index);
protected:
UASInterface* mav; ///< The current MAV

Write Preview
Loading…
Cancel
Save