From 2a47f4baca824ce3a348b0f015bdf6ae921c3134 Mon Sep 17 00:00:00 2001 From: andrew Date: Wed, 20 Oct 2010 05:04:19 +0000 Subject: [PATCH] more modifications to UI for ccpm mixing. will now read back mixer settings from the flight hardware and try to determine the swashplate config automatically. Looks good but only basic testing performed so far. this has not been tested for flight in it's current form. Use at own risk! git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1994 ebee16cc-31ac-478f-84a7-5cbb03baadba --- ground/src/plugins/config/ccpm.ui | 61 +++-- .../src/plugins/config/configccpmwidget.cpp | 218 +++++++++++++++--- 2 files changed, 220 insertions(+), 59 deletions(-) diff --git a/ground/src/plugins/config/ccpm.ui b/ground/src/plugins/config/ccpm.ui index 4549f571b..d1fb75f6b 100644 --- a/ground/src/plugins/config/ccpm.ui +++ b/ground/src/plugins/config/ccpm.ui @@ -56,7 +56,7 @@ Basic settings - + 0 @@ -517,7 +517,7 @@ Qt::AlignCenter - + 0 @@ -632,13 +632,6 @@ - splitter_2 - ccpmCollectivespinBox - ccpmCollectiveSlider - label_15 - ccpmCollectivespinBox - label_16 - @@ -664,7 +657,7 @@ REVO - + 0 @@ -1053,14 +1046,7 @@ - Throttle Curve 2 - - - - - - - Reset + Blade Pitch Curve @@ -1104,14 +1090,7 @@ - Throttle Curve 1 - - - - - - - Reset + Throttle Curve @@ -1156,8 +1135,14 @@ - 260 - 323 + 243 + 273 + + + + + 243 + 273 @@ -1166,7 +1151,7 @@ - Qt::ScrollBarAlwaysOn + Qt::ScrollBarAlwaysOff Qt::ScrollBarAlwaysOff @@ -1177,6 +1162,18 @@ true + + 25 + + + 25 + + + 25 + + + 25 + 0% @@ -1824,10 +1821,10 @@ - + - 170 + 0 0 421 26 @@ -1893,8 +1890,6 @@ CurveValue3 CurveToGenerate ccpmGenerateCurve - pushButton_3 - pushButton_2 CurveSettings ccpmAdvancedSettingsTable diff --git a/ground/src/plugins/config/configccpmwidget.cpp b/ground/src/plugins/config/configccpmwidget.cpp index e2c481139..36791ab75 100644 --- a/ground/src/plugins/config/configccpmwidget.cpp +++ b/ground/src/plugins/config/configccpmwidget.cpp @@ -129,7 +129,7 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) m_ccpm->ccpmServoZChannel->setCurrentIndex(8); QStringList Types; - Types << "CCPM 2 Servo 90º" << "CCPM 3 Servo 120º" << "CCPM 3 Servo 140º" << "Custom - User Angles" << "Custom - Advanced Settings" ; + Types << "CCPM 2 Servo 90º" << "CCPM 3 Servo 120º" << "CCPM 3 Servo 140º" << "FP 2 Servo 90º" << "Custom - User Angles" << "Custom - Advanced Settings" ; m_ccpm->ccpmType->addItems(Types); m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->count() - 1); requestccpmUpdate(); @@ -181,6 +181,7 @@ void ConfigccpmWidget::UpdateType() { int TypeInt,SingleServoIndex; QString TypeText; + double AdjustmentAngle=0; TypeInt = m_ccpm->ccpmType->count() - m_ccpm->ccpmType->currentIndex()-1; @@ -210,40 +211,80 @@ void ConfigccpmWidget::UpdateType() m_ccpm->ccpmRevoSlider->setEnabled(TypeInt>0); m_ccpm->ccpmREVOspinBox->setEnabled(TypeInt>0); + AdjustmentAngle=SingleServoIndex*90; + + m_ccpm->CurveToGenerate->setEnabled(1); + m_ccpm->CurveSettings->setColumnHidden(1,0); + m_ccpm->widget_2->setVisible(1); + m_ccpm->customThrottleCurve2Value->setVisible(1); + m_ccpm->label_41->setVisible(1); //set values for pre defined heli types if (TypeText.compare(QString("CCPM 2 Servo 90º"), Qt::CaseInsensitive)==0) { - m_ccpm->ccpmAngleW->setValue(0); - m_ccpm->ccpmAngleX->setValue(90); + m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); + m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); m_ccpm->ccpmAngleY->setValue(0); m_ccpm->ccpmAngleZ->setValue(0); m_ccpm->ccpmAngleY->setEnabled(0); m_ccpm->ccpmAngleZ->setEnabled(0); + m_ccpm->ccpmServoYChannel->setCurrentIndex(8); + m_ccpm->ccpmServoZChannel->setCurrentIndex(8); m_ccpm->ccpmServoYChannel->setEnabled(0); m_ccpm->ccpmServoZChannel->setEnabled(0); - m_ccpm->ccpmCorrectionAngle->setValue(SingleServoIndex*90); + m_ccpm->ccpmCorrectionAngle->setValue(0); + } if (TypeText.compare(QString("CCPM 3 Servo 120º"), Qt::CaseInsensitive)==0) { - m_ccpm->ccpmAngleW->setValue(0); - m_ccpm->ccpmAngleX->setValue(120); - m_ccpm->ccpmAngleY->setValue(240); + m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); + m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 120,360)); + m_ccpm->ccpmAngleY->setValue(fmod(AdjustmentAngle + 240,360)); m_ccpm->ccpmAngleZ->setValue(0); m_ccpm->ccpmAngleZ->setEnabled(0); + m_ccpm->ccpmServoZChannel->setCurrentIndex(8); m_ccpm->ccpmServoZChannel->setEnabled(0); - m_ccpm->ccpmCorrectionAngle->setValue(SingleServoIndex*90); + m_ccpm->ccpmCorrectionAngle->setValue(0); + } if (TypeText.compare(QString("CCPM 3 Servo 140º"), Qt::CaseInsensitive)==0) { - m_ccpm->ccpmAngleW->setValue(0); - m_ccpm->ccpmAngleX->setValue(140); - m_ccpm->ccpmAngleY->setValue(220); + m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); + m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 140,360)); + m_ccpm->ccpmAngleY->setValue(fmod(AdjustmentAngle + 220,360)); m_ccpm->ccpmAngleZ->setValue(0); m_ccpm->ccpmAngleZ->setEnabled(0); + m_ccpm->ccpmServoZChannel->setCurrentIndex(8); m_ccpm->ccpmServoZChannel->setEnabled(0); - m_ccpm->ccpmCorrectionAngle->setValue(SingleServoIndex*90); + m_ccpm->ccpmCorrectionAngle->setValue(0); + } + if (TypeText.compare(QString("FP 2 Servo 90º"), Qt::CaseInsensitive)==0) + { + m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); + m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); + m_ccpm->ccpmAngleY->setValue(0); + m_ccpm->ccpmAngleZ->setValue(0); + m_ccpm->ccpmAngleY->setEnabled(0); + m_ccpm->ccpmAngleZ->setEnabled(0); + m_ccpm->ccpmServoYChannel->setCurrentIndex(8); + m_ccpm->ccpmServoZChannel->setCurrentIndex(8); + m_ccpm->ccpmServoYChannel->setEnabled(0); + m_ccpm->ccpmServoZChannel->setEnabled(0); + m_ccpm->ccpmCorrectionAngle->setValue(0); + + m_ccpm->ccpmCollectivespinBox->setEnabled(0); + m_ccpm->ccpmCollectiveSlider->setEnabled(0); + m_ccpm->ccpmCollectivespinBox->setValue(0); + m_ccpm->ccpmCollectiveSlider->setValue(0); + m_ccpm->CurveToGenerate->setCurrentIndex(0); + m_ccpm->CurveToGenerate->setEnabled(0); + m_ccpm->CurveSettings->setColumnHidden(1,1); + m_ccpm->widget_2->setVisible(0); + m_ccpm->customThrottleCurve2Value->setVisible(0); + m_ccpm->label_41->setVisible(0); + } + //update UI @@ -495,7 +536,7 @@ void ConfigccpmWidget::UpdateMixer() data.Mixer0Vector[4] = 0;//Yaw */ - if ((MixerChannelData[i]<8)&&(ThisEnable[i])) + if ((MixerChannelData[i]<8)&&((ThisEnable[i])||(i<2))) { m_ccpm->ccpmAdvancedSettingsTable->item(i,0)->setText(QString("%1").arg( MixerChannelData[i] )); //config the vector @@ -554,11 +595,15 @@ void ConfigccpmWidget::UpdateMixer() */ void ConfigccpmWidget::requestccpmUpdate() { +#define MaxAngleError 2 int MixerDataFromHeli[8][5]; QString MixerOutputType[8]; - int EngineChannel,TailRotorChannel,ServoChannels[4],ServoAngles[4],ServoCurve2[4]; + int EngineChannel,TailRotorChannel,ServoChannels[4],ServoAngles[4],SortAngles[4],CalcAngles[4],ServoCurve2[4]; int NumServos=0; double Collective=0.0; + double a1,a2; + int HeadRotation,temp; + int isCCPM=0; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -591,7 +636,8 @@ void ConfigccpmWidget::requestccpmUpdate() { ServoChannels[j]=8; ServoCurve2[j]=0; - ServoAngles[j]=350; + ServoAngles[j]=0; + SortAngles[j]=j; } NumServos=0; @@ -643,22 +689,21 @@ void ConfigccpmWidget::requestccpmUpdate() } } - m_ccpm->ccpmServoWChannel->setCurrentIndex(ServoChannels[0]); - m_ccpm->ccpmServoXChannel->setCurrentIndex(ServoChannels[1]); - m_ccpm->ccpmServoYChannel->setCurrentIndex(ServoChannels[2]); - m_ccpm->ccpmServoZChannel->setCurrentIndex(ServoChannels[3]); - m_ccpm->ccpmAngleW->setValue(ServoAngles[0]); - m_ccpm->ccpmAngleX->setValue(ServoAngles[1]); - m_ccpm->ccpmAngleY->setValue(ServoAngles[2]); - m_ccpm->ccpmAngleZ->setValue(ServoAngles[3]); + + + + //just call it user angles for now.... + m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("Custom - User Angles")); if (NumServos>1) { if((ServoCurve2[0]==0)&&(ServoCurve2[1]==0)&&(ServoCurve2[2]==0)&&(ServoCurve2[3]==0)) { //fixed pitch heli + isCCPM=0; m_ccpm->ccpmCollectiveSlider->setValue(0); + Collective = 0.0; } if(ServoCurve2[0]==ServoCurve2[1]) { @@ -666,13 +711,12 @@ void ConfigccpmWidget::requestccpmUpdate() { if ((NumServos<4)||(ServoCurve2[2]==ServoCurve2[3])) {//all the servos have the same ThrottleCurve2 setting so this must be a CCPM config + isCCPM=1; Collective = ((double)ServoCurve2[0]*100.00)/127.00; m_ccpm->ccpmCollectiveSlider->setValue((int)Collective); m_ccpm->ccpmCollectivespinBox->setValue((int)Collective); - //just call it user angles for now.... - m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("Custom - User Angles")); - } + } } } } @@ -681,6 +725,119 @@ void ConfigccpmWidget::requestccpmUpdate() m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("Custom - Advanced Settings")); } + HeadRotation=0; + //calculate the angles + for(j=0;jccpmSingleServo->setCurrentIndex(HeadRotation); + + //calculate the un rotated angles + for(j=0;j CalcAngles[SortAngles[j+1]]) + {//swap the sorted angles + temp = SortAngles[j]; + SortAngles[j]=SortAngles[j+1]; + SortAngles[j+1]=temp; + } + + } + + m_ccpm->ccpmAngleW->setValue(ServoAngles[SortAngles[0]]); + m_ccpm->ccpmAngleX->setValue(ServoAngles[SortAngles[1]]); + m_ccpm->ccpmAngleY->setValue(ServoAngles[SortAngles[2]]); + m_ccpm->ccpmAngleZ->setValue(ServoAngles[SortAngles[3]]); + + m_ccpm->ccpmServoWChannel->setCurrentIndex(ServoChannels[SortAngles[0]]); + m_ccpm->ccpmServoXChannel->setCurrentIndex(ServoChannels[SortAngles[1]]); + m_ccpm->ccpmServoYChannel->setCurrentIndex(ServoChannels[SortAngles[2]]); + m_ccpm->ccpmServoZChannel->setCurrentIndex(ServoChannels[SortAngles[3]]); + + + //Types << "CCPM 2 Servo 90º" << "CCPM 3 Servo 120º" << "CCPM 3 Servo 140º" << "FP 2 Servo 90º" << "Custom - User Angles" << "Custom - Advanced Settings" ; + + + //check this against known combinations + if (NumServos==2) + { + if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 2 Servo 90º")); + UpdateType(); + } + else + { + m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 2 Servo 90º")); + UpdateType(); + } + + } + } + if (NumServos==3) + { + if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 3 Servo 120º")); + UpdateType(); + + } + else + { + m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 3 Servo 120º")); + UpdateType(); + } + + } + else if ((fabs(CalcAngles[SortAngles[0]])ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("CCPM 3 Servo 140º")); + UpdateType(); + } + else + { + m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->findText("FP 3 Servo 140º")); + UpdateType(); + } + + } + + } + if (NumServos==4) + { + + } + + //get the settings for the curve from the mixer settings @@ -723,6 +880,15 @@ void ConfigccpmWidget::sendccpmUpdate() UpdateMixer(); + //clear the output types + for (i=0;i<8;i++) + { + field = obj->getField(tr( "Mixer%1Type" ).arg( i )); + //clear the mixer type + field->setValue("Disabled"); + } + + //go through the user data and update the mixer matrix for (i=0;i<6;i++) {