From c26e33964ab8d64beabdc7d2610ed08f94690194 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 13 Jul 2014 13:58:18 +0200 Subject: [PATCH] OP-1063 Multi_config_Hexa_fixes Added OctoX, define new mixer table for Octo(p) and OctoX. New motor definition for OctoX (NNE, ENE...) Fixes for svg frames. Defined default settings for OctoP/X in setupwizard (but disabled) --- flight/libraries/sanitycheck.c | 1 + .../VtolPathFollower/vtolpathfollower.c | 3 +- .../configmultirotorwidget.cpp | 149 +++++++++++++++--- .../config/cfg_vehicletypes/vehicleconfig.h | 9 ++ .../config/configvehicletypewidget.cpp | 9 +- .../config/images/multirotor-shapes.svg | 24 +-- .../vehicleconfigurationhelper.cpp | 133 +++++++++++++--- .../setupwizard/vehicleconfigurationsource.h | 4 +- shared/uavobjectdefinition/systemsettings.xml | 2 +- 9 files changed, 269 insertions(+), 65 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index c2a2ea0cf..365e1af7b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -265,6 +265,7 @@ FrameType_t GetCurrentFrameType() case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: + case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index a86f0fec2..bae101487 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -190,7 +190,8 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) - && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH)) { + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH)) + && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index c834cd128..e04cd986c 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -55,6 +55,32 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions() // get the gui config data GUIConfigDataUnion configData = getConfigData(); multiGUISettingsStruct multi = configData.multi; + /* Octocopter X motor definition */ + if (multi.VTOLMotorNNE > 0 && multi.VTOLMotorNNE <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorNNE - 1] = QString("VTOLMotorNNE"); + } + if (multi.VTOLMotorENE > 0 && multi.VTOLMotorENE <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorENE - 1] = QString("VTOLMotorENE"); + } + if (multi.VTOLMotorESE > 0 && multi.VTOLMotorESE <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorESE - 1] = QString("VTOLMotorESE"); + } + if (multi.VTOLMotorSSE > 0 && multi.VTOLMotorSSE <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorSSE - 1] = QString("VTOLMotorSSE"); + } + if (multi.VTOLMotorSSW > 0 && multi.VTOLMotorSSW <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorSSW - 1] = QString("VTOLMotorSSW"); + } + if (multi.VTOLMotorWSW > 0 && multi.VTOLMotorWSW <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorWSW - 1] = QString("VTOLMotorWSW"); + } + if (multi.VTOLMotorWNW > 0 && multi.VTOLMotorWNW <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorWNW - 1] = QString("VTOLMotorWNW"); + } + if (multi.VTOLMotorNNW > 0 && multi.VTOLMotorNNW <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { + channelDesc[multi.VTOLMotorNNW - 1] = QString("VTOLMotorNNW"); + } + // End OctocopterX if (multi.VTOLMotorN > 0 && multi.VTOLMotorN <= ConfigMultiRotorWidget::CHANNEL_NUMELEM) { channelDesc[multi.VTOLMotorN - 1] = QString("VTOLMotorN"); @@ -111,7 +137,7 @@ ConfigMultiRotorWidget::ConfigMultiRotorWidget(QWidget *parent) : QStringList multiRotorTypes; multiRotorTypes << "Tricopter Y" << "Quad +" << "Quad X" << "Hexacopter" << "Hexacopter X" << "Hexacopter H" << "Hexacopter Y6" - << "Octocopter" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; + << "Octocopter" << "Octocopter X" << "Octocopter V" << "Octo Coax +" << "Octo Coax X"; m_aircraft->multirotorFrameType->addItems(multiRotorTypes); // Set default model to "Quad X" @@ -185,9 +211,15 @@ void ConfigMultiRotorWidget::setupUI(QString frameType) } else if (frameType == "Octo" || frameType == "Octocopter") { setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter")); - m_aircraft->mrRollMixLevel->setValue(33); - m_aircraft->mrPitchMixLevel->setValue(33); - setYawMixLevel(25); + m_aircraft->mrRollMixLevel->setValue(100); // Do not alter mixer matrix + m_aircraft->mrPitchMixLevel->setValue(100); // All mixers RPY levels = 100% + setYawMixLevel(100); + } else if (frameType == "OctoX" || frameType == "Octocopter X") { + setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter X")); + + m_aircraft->mrRollMixLevel->setValue(100); // Do not alter mixer matrix + m_aircraft->mrPitchMixLevel->setValue(100); // All mixers RPY levels = 100% + setYawMixLevel(100); } else if (frameType == "OctoV" || frameType == "Octocopter V") { setComboCurrentIndex(m_aircraft->multirotorFrameType, m_aircraft->multirotorFrameType->findText("Octocopter V")); @@ -248,6 +280,8 @@ void ConfigMultiRotorWidget::setupEnabledControls(QString frameType) enableComboBoxes(this, CHANNELBOXNAME, 6, true); } else if (frameType == "Octo" || frameType == "Octocopter") { enableComboBoxes(this, CHANNELBOXNAME, 8, true); + } else if (frameType == "OctoX" || frameType == "Octocopter X") { + enableComboBoxes(this, CHANNELBOXNAME, 8, true); } else if (frameType == "OctoV" || frameType == "Octocopter V") { enableComboBoxes(this, CHANNELBOXNAME, 8, true); } else if (frameType == "OctoCoaxP" || frameType == "Octo Coax +") { @@ -288,6 +322,14 @@ void ConfigMultiRotorWidget::resetActuators(GUIConfigDataUnion *configData) configData->multi.VTOLMotorW = 0; configData->multi.VTOLMotorNW = 0; configData->multi.TRIYaw = 0; + configData->multi.VTOLMotorNNE = 0; + configData->multi.VTOLMotorENE = 0; + configData->multi.VTOLMotorESE = 0; + configData->multi.VTOLMotorSSE = 0; + configData->multi.VTOLMotorSSW = 0; + configData->multi.VTOLMotorWSW = 0; + configData->multi.VTOLMotorWNW = 0; + configData->multi.VTOLMotorNNW = 0; } /** @@ -380,7 +422,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); //get motor 2 value for Yaw and Roll - channel += 1; + channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); @@ -409,7 +451,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); //get motor 2 value for Yaw and Roll - channel += 1; + channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); @@ -492,11 +534,11 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); setYawMixLevel(-qRound(value / 1.27)); - // change channels - channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; + // Get M3 Roll value + channel = m_aircraft->multiMotorChannelBox3->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); - } else if (frameType == "OctoV") { + } else if (frameType == "OctoV") { double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); @@ -520,6 +562,34 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); } } + } else if (frameType == "OctoX") { + // Motors 1 to 8 are NNE / ENE / ESE / etc + setComboCurrentIndex(m_aircraft->multiMotorChannelBox1, multi.VTOLMotorNNE); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox2, multi.VTOLMotorENE); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox3, multi.VTOLMotorESE); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox4, multi.VTOLMotorSSE); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox5, multi.VTOLMotorSSW); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox6, multi.VTOLMotorWSW); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox7, multi.VTOLMotorWNW); + setComboCurrentIndex(m_aircraft->multiMotorChannelBox8, multi.VTOLMotorNNW); + + + // Now, read the 1st mixer R/P/Y levels and initialize the mix sliders. + // This assumes that all vectors are identical - if not, the user should use the + // "custom" setting. + int channel = m_aircraft->multiMotorChannelBox1->currentIndex() - 1; + if (channel > -1) { + double value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH); + m_aircraft->mrPitchMixLevel->setValue(qRound(value / 1.27)); + + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); + setYawMixLevel(-qRound(value / 1.27)); + + // Get M2 Roll value + channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; + value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); + m_aircraft->mrRollMixLevel->setValue(-qRound(value / 1.27)); + } } else if (frameType == "OctoCoaxX") { // Motors 1 to 8 are N / NE / E / etc setComboCurrentIndex(m_aircraft->multiMotorChannelBox1, multi.VTOLMotorNW); @@ -657,16 +727,40 @@ QString ConfigMultiRotorWidget::updateConfigObjectsFromWidgets() << "VTOLMotorW" << "VTOLMotorNW"; setupMotors(motorList); // Motor 1 to 8: - // pitch roll yaw + // pitch roll yaw (OctoP) double mixerMatrix[8][3] = { - { 1, 0, -1 }, - { 1, -1, 1 }, - { 0, -1, -1 }, - { -1, -1, 1 }, - { -1, 0, -1 }, - { -1, 1, 1 }, - { 0, 1, -1 }, - { 1, 1, 1 } + { 1 , 0 , -1 }, + { 0.71,-0.71, 1 }, + { 0 ,-1 , -1 }, + { -0.71,-0.71, 1 }, + { -1 , 0 , -1 }, + { -0.71, 0.71, 1 }, + { 0 , 1 , -1 }, + { 0.71, 0.71, 1 } + }; + setupMultiRotorMixer(mixerMatrix); + m_aircraft->mrStatusLabel->setText("Configuration OK"); + } else if (m_aircraft->multirotorFrameType->currentText() == "Octocopter X") { + airframeType = "OctoX"; + + // Show any config errors in GUI + if (throwConfigError(8)) { + return airframeType; + } + motorList << "VTOLMotorNNE" << "VTOLMotorENE" << "VTOLMotorESE" << "VTOLMotorSSE" << "VTOLMotorSSW" << "VTOLMotorWSW" + << "VTOLMotorWNW" << "VTOLMotorNNW"; + setupMotors(motorList); + // Motor 1 to 8: + // pitch roll yaw (OctoX) + double mixerMatrix[8][3] = { + { 1 ,-0.41, -1 }, + { 0.41, -1 , 1 }, + { -0.41, -1 , -1 }, + { -1 ,-0.41, 1 }, + { -1 , 0.41, -1 }, + { -0.41, 1 , 1 }, + { 0.41, 1 , -1 }, + { 1 , 0.41, 1 } }; setupMultiRotorMixer(mixerMatrix); m_aircraft->mrStatusLabel->setText("Configuration OK"); @@ -828,6 +922,8 @@ void ConfigMultiRotorWidget::updateAirframe(QString frameType) elementId = "hexa-coax"; } else if (frameType == "Octo" || frameType == "Octocopter") { elementId = "quad-octo"; + } else if (frameType == "OctoX" || frameType == "Octocopter X") { + elementId = "quad-octo-X"; } else if (frameType == "OctoV" || frameType == "Octocopter V") { elementId = "quad-octo-v"; } else if (frameType == "OctoCoaxP" || frameType == "Octo Coax +") { @@ -899,6 +995,23 @@ void ConfigMultiRotorWidget::setupMotors(QList motorList) configData.multi.VTOLMotorW = index; } else if (motor == QString("VTOLMotorNW")) { configData.multi.VTOLMotorNW = index; + //OctoX + } else if (motor == QString("VTOLMotorNNE")) { + configData.multi.VTOLMotorNNE = index; + } else if (motor == QString("VTOLMotorENE")) { + configData.multi.VTOLMotorENE = index; + } else if (motor == QString("VTOLMotorESE")) { + configData.multi.VTOLMotorESE = index; + } else if (motor == QString("VTOLMotorSSE")) { + configData.multi.VTOLMotorSSE = index; + } else if (motor == QString("VTOLMotorSSW")) { + configData.multi.VTOLMotorSSW = index; + } else if (motor == QString("VTOLMotorWSW")) { + configData.multi.VTOLMotorWSW = index; + } else if (motor == QString("VTOLMotorWNW")) { + configData.multi.VTOLMotorWNW = index; + } else if (motor == QString("VTOLMotorNNW")) { + configData.multi.VTOLMotorNNW = index; } } setConfigData(configData); diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h index 26c468601..7ffae4cad 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.h @@ -42,6 +42,15 @@ typedef struct { uint VTOLMotorNE : 4; uint VTOLMotorSW : 4; uint VTOLMotorSE : 4; // 32 bits + /* OctoX */ + uint VTOLMotorNNE : 4; + uint VTOLMotorENE : 4; + uint VTOLMotorESE : 4; + uint VTOLMotorSSE : 4; + uint VTOLMotorSSW : 4; + uint VTOLMotorWSW : 4; + uint VTOLMotorWNW : 4; + uint VTOLMotorNNW : 4; // 32 bits uint TRIYaw : 4; quint32 padding : 28; // 64 bits quint32 padding1; diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index 0c881e231..5cb9e8547 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -85,6 +85,7 @@ QStringList ConfigVehicleTypeWidget::getChannelDescriptions() case SystemSettings::AIRFRAMETYPE_OCTOCOAXX: case SystemSettings::AIRFRAMETYPE_OCTOCOAXP: case SystemSettings::AIRFRAMETYPE_OCTO: + case SystemSettings::AIRFRAMETYPE_OCTOX: case SystemSettings::AIRFRAMETYPE_HEXAX: case SystemSettings::AIRFRAMETYPE_HEXACOAX: case SystemSettings::AIRFRAMETYPE_HEXA: @@ -263,9 +264,11 @@ int ConfigVehicleTypeWidget::frameCategory(QString frameType) } else if (frameType == "Tri" || frameType == "Tricopter Y" || frameType == "QuadX" || frameType == "Quad X" || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" || frameType == "HexaX" || frameType == "Hexacopter X" || frameType == "HexaCoax" - || frameType == "HexaH" || frameType == "Hexacopter H" - || frameType == "Hexacopter Y6" || frameType == "Octo" || frameType == "Octocopter" || frameType == "OctoV" - || frameType == "Octocopter V" || frameType == "OctoCoaxP" || frameType == "Octo Coax +" + || frameType == "HexaH" || frameType == "Hexacopter H" || frameType == "Hexacopter Y6" + || frameType == "Octo" || frameType == "Octocopter" + || frameType == "OctoX" || frameType == "Octocopter X" + || frameType == "OctoV" || frameType == "Octocopter V" + || frameType == "OctoCoaxP" || frameType == "Octo Coax +" || frameType == "OctoCoaxX" || frameType == "Octo Coax X") { return ConfigVehicleTypeWidget::MULTIROTOR; } else if (frameType == "HeliCP") { diff --git a/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg index 743fc7fa1..fae42213c 100644 --- a/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/multirotor-shapes.svg @@ -29,22 +29,16 @@ inkscape:window-height="928" id="namedview6635" showgrid="false" -<<<<<<< HEAD - inkscape:zoom="0.17071524" - inkscape:cx="2032.6246" - inkscape:cy="2727.8341" -======= - inkscape:zoom="1.5262997" - inkscape:cx="1120.2257" - inkscape:cy="2126.7661" ->>>>>>> 0c0ae3641370f2d0db85ab231b0967f810ea5d3e + inkscape:zoom="0.46421413" + inkscape:cx="3322.5456" + inkscape:cy="3589.5054" inkscape:window-x="0" inkscape:window-y="27" inkscape:window-maximized="1" inkscape:current-layer="svg4183" />image/svg+xml>>>>>> 0c0ae3641370f2d0db85ab231b0967f810ea5d3e id="g4327" transform="matrix(1.25,0,0,1.25,-146.76331,-311.9055)" /> -======= - inkscape:connector-curvature="0" /> ->>>>>>> 0c0ae3641370f2d0db85ab231b0967f810ea5d3e + d="m 3062.2588,1271.4951 l 2.557,0 c 0.279,0 0.5,-0.107 0.695,-0.297 c 0.1,-0.088 0.176,-0.199 0.215,-0.314 c 0.051,-0.118 0.084,-0.246 0.084,-0.387 l 0,-0.914 c 0,-0.135 -0.033,-0.266 -0.084,-0.395 c -0.039,-0.107 -0.115,-0.209 -0.215,-0.298 c -0.195,-0.194 -0.416,-0.299 -0.695,-0.299 l -2.557,0 c -0.275,0 -0.504,0.105 -0.693,0.299 c -0.104,0.089 -0.174,0.191 -0.225,0.298 c -0.039,0.129 -0.078,0.26 -0.078,0.395 l 0,0.914 c 0,0.141 0.039,0.269 0.078,0.387 c 0.051,0.115 0.121,0.226 0.225,0.314 c 0.189,0.19 0.418,0.297 0.693,0.297 m -12.41,0 l 2.558,0 c 0.276,0 0.497,-0.107 0.688,-0.297 c 0.191,-0.187 0.309,-0.426 0.309,-0.701 l 0,-0.914 c 0,-0.277 -0.118,-0.526 -0.309,-0.693 c -0.184,-0.192 -0.41,-0.299 -0.676,-0.299 l -3.566,0 l 0,0.992 l 0,0.521 l 0,0.393 c 0,0.141 0.035,0.269 0.084,0.387 c 0.051,0.115 0.129,0.226 0.228,0.314 c 0.176,0.19 0.403,0.297 0.684,0.297 m -15.022,0 l 2.551,0 c 0.287,0 0.506,-0.107 0.701,-0.297 c 0.096,-0.088 0.166,-0.199 0.213,-0.314 c 0.053,-0.118 0.082,-0.246 0.082,-0.379 l -4.541,0 c 0,0.133 0.033,0.261 0.08,0.379 c 0.051,0.115 0.121,0.226 0.219,0.314 c 0.189,0.19 0.426,0.297 0.695,0.297 m -7.509,0 l 2.537,0 c 0.281,0 0.519,-0.107 0.715,-0.297 c 0.197,-0.187 0.281,-0.426 0.281,-0.701 l 0,-0.914 c 0,-0.277 -0.084,-0.526 -0.281,-0.693 c -0.196,-0.194 -0.434,-0.299 -0.715,-0.299 l -1.893,0 l -1.641,0 l 0,0.992 l 0,0.521 l 0,0.393 c 0,0.141 0.02,0.269 0.073,0.387 c 0.062,0.115 0.135,0.226 0.224,0.314 c 0.18,0.19 0.42,0.297 0.7,0.297 m -7.518,0 l 2.539,0 c 0.277,0 0.522,-0.107 0.709,-0.297 c 0.088,-0.088 0.158,-0.199 0.215,-0.314 c 0.051,-0.118 0.074,-0.246 0.074,-0.387 l 0,-0.914 c 0,-0.135 -0.023,-0.266 -0.074,-0.395 c -0.057,-0.107 -0.127,-0.209 -0.215,-0.298 c -0.187,-0.194 -0.432,-0.299 -0.709,-0.299 l -0.318,0 l -2.221,0 c -0.281,0 -0.514,0.105 -0.713,0.299 c -0.082,0.089 -0.158,0.191 -0.209,0.298 c -0.057,0.129 -0.074,0.26 -0.074,0.395 l 0,0.914 c 0,0.141 0.017,0.269 0.074,0.387 c 0.051,0.115 0.127,0.226 0.209,0.314 c 0.199,0.19 0.432,0.297 0.713,0.297 m 42.391,1.041 c -0.278,0 -0.543,-0.049 -0.795,-0.166 c -0.233,-0.088 -0.453,-0.242 -0.619,-0.42 c -0.194,-0.189 -0.34,-0.406 -0.457,-0.652 c -0.106,-0.254 -0.141,-0.52 -0.141,-0.793 l 0,-0.93 c 0,-0.281 0.035,-0.547 0.141,-0.795 c 0.117,-0.242 0.263,-0.455 0.457,-0.642 c 0.166,-0.178 0.386,-0.332 0.619,-0.426 c 0.252,-0.115 0.517,-0.16 0.795,-0.16 l 2.693,0 c 0.277,0 0.543,0.045 0.791,0.16 c 0.236,0.094 0.459,0.248 0.637,0.426 c 0.189,0.187 0.334,0.4 0.445,0.642 c 0.096,0.248 0.147,0.514 0.147,0.795 l 0,0.93 c 0,0.273 -0.051,0.539 -0.147,0.793 c -0.111,0.246 -0.256,0.463 -0.445,0.652 c -0.178,0.178 -0.401,0.332 -0.637,0.42 c -0.248,0.117 -0.514,0.166 -0.791,0.166 l -2.693,0 z m -27.428,0 c -0.274,0 -0.539,-0.049 -0.797,-0.166 c -0.236,-0.088 -0.451,-0.242 -0.625,-0.42 c -0.186,-0.189 -0.34,-0.406 -0.447,-0.652 c -0.108,-0.252 -0.156,-0.508 -0.156,-0.785 l 0,-0.93 c 0,-0.277 0.048,-0.541 0.156,-0.795 c 0.107,-0.244 0.261,-0.463 0.447,-0.65 c 0.174,-0.178 0.389,-0.332 0.625,-0.426 c 0.258,-0.115 0.523,-0.16 0.797,-0.16 l 4.111,0 l 0,1.039 l -4.047,0 c -0.269,0 -0.506,0.105 -0.695,0.299 c -0.074,0.078 -0.133,0.16 -0.18,0.257 c -0.058,0.096 -0.091,0.203 -0.107,0.303 l 5.627,0 l 0,1.063 c 0,0.277 -0.051,0.533 -0.145,0.785 c -0.117,0.246 -0.254,0.463 -0.449,0.652 c -0.182,0.178 -0.4,0.332 -0.644,0.42 c -0.233,0.117 -0.5,0.166 -0.791,0.166 l -2.68,0 z m -15.031,0 c -0.291,0 -0.555,-0.049 -0.793,-0.166 c -0.25,-0.088 -0.459,-0.242 -0.645,-0.42 c -0.183,-0.189 -0.336,-0.406 -0.445,-0.652 c -0.1,-0.254 -0.153,-0.52 -0.153,-0.793 l 0,-0.93 c 0,-0.281 0.053,-0.547 0.153,-0.795 c 0.109,-0.242 0.262,-0.455 0.445,-0.642 c 0.186,-0.178 0.395,-0.332 0.645,-0.426 c 0.238,-0.115 0.502,-0.16 0.793,-0.16 l 2.011,0 l 0.664,0 l 2.803,0 l 0,-1.989 l 1.111,0 l 0,1.989 l 0.928,0 l 0.36,0 l 2.324,0 c 0.283,0 0.543,0.045 0.783,0.16 c 0.252,0.094 0.463,0.248 0.654,0.426 c 0.192,0.187 0.334,0.4 0.436,0.642 c 0.099,0.248 0.158,0.514 0.158,0.795 l 0,0.93 c 0,0.273 -0.059,0.539 -0.158,0.793 c -0.102,0.246 -0.244,0.463 -0.436,0.652 c -0.191,0.178 -0.402,0.332 -0.654,0.42 c -0.24,0.117 -0.5,0.166 -0.783,0.166 l -2.684,0 c -0.281,0 -0.545,-0.049 -0.793,-0.166 c -0.244,-0.088 -0.455,-0.242 -0.642,-0.42 c -0.186,-0.189 -0.338,-0.406 -0.44,-0.652 c -0.107,-0.254 -0.164,-0.52 -0.164,-0.793 l 0,-1.914 l -1.017,0 c 0.027,0.062 0.068,0.127 0.087,0.189 c 0.112,0.248 0.165,0.514 0.165,0.795 l 0,0.93 c 0,0.273 -0.053,0.539 -0.165,0.793 c -0.091,0.246 -0.244,0.463 -0.423,0.652 c -0.192,0.178 -0.407,0.332 -0.653,0.42 c -0.252,0.117 -0.517,0.166 -0.797,0.166 l -2.675,0 z m 35.543,-4.984 l 1.105,0 l 0,5 l -1.105,0 l 0,-5 z m -13.004,5 c -0.274,0 -0.539,-0.059 -0.791,-0.182 c -0.229,-0.088 -0.446,-0.242 -0.625,-0.42 c -0.186,-0.189 -0.336,-0.406 -0.451,-0.652 c -0.096,-0.252 -0.139,-0.508 -0.139,-0.785 l 0,-2.961 l 1.086,0 l 0,2.953 c 0,0.273 0.113,0.513 0.301,0.707 c 0.08,0.09 0.197,0.158 0.302,0.213 c 0.121,0.052 0.244,0.074 0.381,0.074 l 2.569,0 c 0.132,0 0.263,-0.022 0.369,-0.074 c 0.121,-0.055 0.244,-0.123 0.314,-0.213 c 0.197,-0.194 0.301,-0.434 0.301,-0.707 l 0,-2.953 l 0.307,0 l 0.785,0 l 0.777,0 l 0,-1.989 l 1.096,0 l 0,1.989 l 0.927,0 l 2.299,0 l 0.407,0 c 0.265,0 0.531,0.045 0.783,0.16 c 0.23,0.094 0.451,0.248 0.631,0.426 c 0.183,0.187 0.338,0.4 0.445,0.642 c 0.103,0.248 0.139,0.514 0.139,0.795 l 0,0.93 c 0,0.273 -0.036,0.539 -0.139,0.793 c -0.107,0.246 -0.262,0.463 -0.445,0.652 c -0.18,0.178 -0.401,0.332 -0.631,0.42 c -0.252,0.117 -0.518,0.166 -0.783,0.166 l -2.706,0 c -0.279,0 -0.544,-0.049 -0.792,-0.166 c -0.225,-0.088 -0.438,-0.242 -0.625,-0.42 c -0.19,-0.189 -0.34,-0.406 -0.456,-0.652 c -0.099,-0.254 -0.15,-0.52 -0.15,-0.793 l 0,-1.914 l -0.777,0 l 0,1.914 l 0,0.01 c 0,0.277 -0.053,0.533 -0.139,0.785 c -0.119,0.246 -0.262,0.463 -0.451,0.652 c -0.184,0.178 -0.406,0.332 -0.643,0.42 c -0.23,0.123 -0.496,0.182 -0.777,0.182 l -2.699,0 z m 25.617,1.429 l 0,-1.22 l -0.49,0 l 0,-1.053 l 0.49,0 l 0,-2.133 c 0,-0.281 0.06,-0.547 0.154,-0.795 c 0.112,-0.242 0.252,-0.455 0.438,-0.642 c 0.195,-0.178 0.4,-0.332 0.652,-0.426 c 0.236,-0.115 0.502,-0.16 0.793,-0.16 l 0.791,0 l 0,1.039 l -0.777,0 c -0.27,0.025 -0.504,0.129 -0.664,0.32 c -0.096,0.078 -0.168,0.191 -0.213,0.307 c -0.049,0.105 -0.065,0.232 -0.065,0.365 l 0,2.125 l 1.719,0 l 0,0.223 l 0,0.83 l -1.719,0 l 0,1.22 l -1.109,0 z m -10.752,0.563 l 0,-4.961 c 0,-0.277 0.06,-0.541 0.162,-0.795 c 0.1,-0.244 0.24,-0.463 0.434,-0.639 c 0.197,-0.183 0.398,-0.332 0.66,-0.437 c 0.234,-0.115 0.49,-0.16 0.777,-0.16 l 0.221,0 l 0,1.035 l -0.201,0 c -0.262,0.025 -0.479,0.129 -0.664,0.32 c -0.088,0.078 -0.157,0.191 -0.198,0.307 c -0.064,0.105 -0.068,0.232 -0.068,0.365 l 0,4.961 l -1.123,0 z m -1.861,-1.096 l 1.105,0 l 0,1.096 l -1.105,0 l 0,-1.096 z" /> \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index f2fcb1e7b..2ebcdedcd 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -206,6 +206,7 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() setupHexaCopter(); break; case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: @@ -282,6 +283,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: case VehicleConfigurationSource::MULTI_ROTOR_HEXA_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO: + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_V: @@ -1089,62 +1091,71 @@ void VehicleConfigurationHelper::setupOctoCopter() case VehicleConfigurationSource::MULTI_ROTOR_OCTO: { frame = SystemSettings::AIRFRAMETYPE_OCTO; - + // OctoP according to new mixer table and pitch-roll-yaw mixing at 100% + // Pitch Roll Yaw + //M1{ 1 , 0 , -1 }, + //M2{ 0.71,-0.71, 1 }, + //M3{ 0 ,-1 , -1 }, + //M4{ -0.71,-0.71, 1 }, + //M5{ -1 , 0 , -1 }, + //M6{ -0.71, 0.71, 1 }, + //M7{ 0 , 1 , -1 }, + //M8{ 0.71, 0.71, 1 } channels[0].type = MIXER_TYPE_MOTOR; channels[0].throttle1 = 100; channels[0].throttle2 = 0; channels[0].roll = 0; - channels[0].pitch = 33; - channels[0].yaw = -25; + channels[0].pitch = 100; + channels[0].yaw = -100; channels[1].type = MIXER_TYPE_MOTOR; channels[1].throttle1 = 100; channels[1].throttle2 = 0; - channels[1].roll = -33; - channels[1].pitch = 33; - channels[1].yaw = 25; + channels[1].roll = -71; + channels[1].pitch = 71; + channels[1].yaw = 100; channels[2].type = MIXER_TYPE_MOTOR; channels[2].throttle1 = 100; channels[2].throttle2 = 0; - channels[2].roll = -33; + channels[2].roll = -71; channels[2].pitch = 0; - channels[2].yaw = -25; + channels[2].yaw = -100; channels[3].type = MIXER_TYPE_MOTOR; channels[3].throttle1 = 100; channels[3].throttle2 = 0; - channels[3].roll = -33; - channels[3].pitch = -33; - channels[3].yaw = 25; + channels[3].roll = -71; + channels[3].pitch = -71; + channels[3].yaw = 100; channels[4].type = MIXER_TYPE_MOTOR; channels[4].throttle1 = 100; channels[4].throttle2 = 0; channels[4].roll = 0; - channels[4].pitch = -33; - channels[4].yaw = -25; + channels[4].pitch = -100; + channels[4].yaw = -100; channels[5].type = MIXER_TYPE_MOTOR; channels[5].throttle1 = 100; channels[5].throttle2 = 0; - channels[5].roll = 33; - channels[5].pitch = -33; - channels[5].yaw = 25; + channels[5].roll = 71; + channels[5].pitch = -71; + channels[5].yaw = 100; channels[6].type = MIXER_TYPE_MOTOR; channels[6].throttle1 = 100; channels[6].throttle2 = 0; - channels[6].roll = 33; + channels[6].roll = 100; channels[6].pitch = 0; - channels[6].yaw = -25; + channels[6].yaw = -100; channels[7].type = MIXER_TYPE_MOTOR; channels[7].throttle1 = 100; channels[7].throttle2 = 0; - channels[7].roll = 33; - channels[7].pitch = 33; - channels[7].yaw = 25; + channels[7].roll = 71; + channels[7].pitch = 71; + channels[7].yaw = 100; guiSettings.multi.VTOLMotorN = 1; guiSettings.multi.VTOLMotorNE = 2; @@ -1157,6 +1168,86 @@ void VehicleConfigurationHelper::setupOctoCopter() break; } + case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X: + { + frame = SystemSettings::AIRFRAMETYPE_OCTOX; + // OctoX according to new mixer table and pitch-roll-yaw mixing at 100% + // Pitch Roll Yaw + //M1{ 1 ,-0.41, -1 }, + //M2{ 0.41, -1 , 1 }, + //M3{ -0.41, -1 , -1 }, + //M4{ -1 ,-0.41, 1 }, + //M5{ -1 , 0.41, -1 }, + //M6{ -0.41, 1 , 1 }, + //M7{ 0.41, 1 , -1 }, + //M8{ 1 , 0.41, 1 } + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = -41; + channels[0].pitch = 100; + channels[0].yaw = -100; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 41; + channels[1].yaw = 100; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = -100; + channels[2].pitch = -41; + channels[2].yaw = -100; + + channels[3].type = MIXER_TYPE_MOTOR; + channels[3].throttle1 = 100; + channels[3].throttle2 = 0; + channels[3].roll = -41; + channels[3].pitch = -100; + channels[3].yaw = 100; + + channels[4].type = MIXER_TYPE_MOTOR; + channels[4].throttle1 = 100; + channels[4].throttle2 = 0; + channels[4].roll = 41; + channels[4].pitch = -100; + channels[4].yaw = -100; + + channels[5].type = MIXER_TYPE_MOTOR; + channels[5].throttle1 = 100; + channels[5].throttle2 = 0; + channels[5].roll = 100; + channels[5].pitch = -41; + channels[5].yaw = 100; + + channels[6].type = MIXER_TYPE_MOTOR; + channels[6].throttle1 = 100; + channels[6].throttle2 = 0; + channels[6].roll = 100; + channels[6].pitch = 41; + channels[6].yaw = -100; + + channels[7].type = MIXER_TYPE_MOTOR; + channels[7].throttle1 = 100; + channels[7].throttle2 = 0; + channels[7].roll = 41; + channels[7].pitch = 100; + channels[7].yaw = 100; + + guiSettings.multi.VTOLMotorNNE = 1; + guiSettings.multi.VTOLMotorENE = 2; + guiSettings.multi.VTOLMotorESE = 3; + guiSettings.multi.VTOLMotorSSE = 4; + guiSettings.multi.VTOLMotorSSW = 5; + guiSettings.multi.VTOLMotorWSW = 6; + guiSettings.multi.VTOLMotorWNW = 7; + guiSettings.multi.VTOLMotorNNW = 8; + + break; + } case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: { frame = SystemSettings::AIRFRAMETYPE_OCTOCOAXX; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index df286a19c..797283656 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -59,8 +59,8 @@ public: enum VEHICLE_TYPE { VEHICLE_UNKNOWN, VEHICLE_MULTI, VEHICLE_FIXEDWING, VEHICLE_HELI, VEHICLE_SURFACE }; enum VEHICLE_SUB_TYPE { MULTI_ROTOR_UNKNOWN, MULTI_ROTOR_TRI_Y, MULTI_ROTOR_QUAD_X, MULTI_ROTOR_QUAD_PLUS, MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, - MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, - FIXED_WING_VTAIL, HELI_CCPM }; + MULTI_ROTOR_OCTO_X, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, + FIXED_WING_AILERON, FIXED_WING_VTAIL, HELI_CCPM }; enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index 472f474de..ae808f185 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -1,7 +1,7 @@ Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand - +