diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index 3cd43c4a3..5c5da9c6f 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -148,7 +148,8 @@ void ConfigOutputWidget::runChannelTests(bool state) if (state && systemAlarms.Alarm[SystemAlarms::ALARM_ACTUATOR] != SystemAlarms::ALARM_OK) { QMessageBox mbox; - mbox.setText(QString(tr("The actuator module is in an error state. This can also occur because there are no inputs. Please fix these before testing outputs."))); + mbox.setText(QString(tr("The actuator module is in an error state. This can also occur because there are no inputs. " + "Please fix these before testing outputs."))); mbox.setStandardButtons(QMessageBox::Ok); mbox.exec(); @@ -162,7 +163,8 @@ void ConfigOutputWidget::runChannelTests(bool state) // Confirm this is definitely what they want if (state) { QMessageBox mbox; - mbox.setText(QString(tr("This option will start your motors by the amount selected on the sliders regardless of transmitter. It is recommended to remove any blades from motors. Are you sure you want to do this?"))); + mbox.setText(QString(tr("This option will start your motors by the amount selected on the sliders regardless of transmitter." + "It is recommended to remove any blades from motors. Are you sure you want to do this?"))); mbox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); int retval = mbox.exec(); if (retval != QMessageBox::Yes) { diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframeinitialtuningpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframeinitialtuningpage.cpp index a8fcb3dca..60dad2b28 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframeinitialtuningpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframeinitialtuningpage.cpp @@ -85,7 +85,7 @@ bool AirframeInitialTuningPage::validatePage() if (getWizard()->getVehicleTemplate() != NULL) { delete getWizard()->getVehicleTemplate(); } - getWizard()->setVehicleTemplate(new QJsonObject(*templ)); + getWizard()->setVehicleTemplate(templ != NULL ? new QJsonObject(*templ) : NULL); return true; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.cpp new file mode 100644 index 000000000..3428fb794 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.cpp @@ -0,0 +1,102 @@ +/** + ****************************************************************************** + * + * @file EscCalibrationPage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @addtogroup + * @{ + * @addtogroup EscCalibrationPage + * @{ + * @brief + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#include "esccalibrationpage.h" +#include "ui_esccalibrationpage.h" +#include "setupwizard.h" +#include "mixersettings.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "vehicleconfigurationhelper.h" +#include "actuatorsettings.h" + +EscCalibrationPage::EscCalibrationPage(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::EscCalibrationPage), m_isCalibrating(false) +{ + ui->setupUi(this); + connect(ui->startStopButton, SIGNAL(clicked()), this, SLOT(startStopButtonClicked())); +} + +EscCalibrationPage::~EscCalibrationPage() +{ + delete ui; +} + +bool EscCalibrationPage::validatePage() +{ + return true; +} + +void EscCalibrationPage::enableButtons(bool enable) +{ + getWizard()->button(QWizard::NextButton)->setEnabled(enable); + getWizard()->button(QWizard::CancelButton)->setEnabled(enable); + getWizard()->button(QWizard::BackButton)->setEnabled(enable); + getWizard()->button(QWizard::CustomButton1)->setEnabled(enable); + QApplication::processEvents(); +} + +void EscCalibrationPage::startStopButtonClicked() +{ + if (!m_isCalibrating) { + m_isCalibrating = true; + ui->startStopButton->setEnabled(false); + enableButtons(false); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *uavoManager = pm->getObject(); + Q_ASSERT(uavoManager); + MixerSettings *mSettings = MixerSettings::GetInstance(uavoManager); + Q_ASSERT(mSettings); + QString mixerTypePattern = "Mixer%1Type"; + for (int i = 0; i < ActuatorSettings::CHANNELADDR_NUMELEM; i++) { + UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i + 1)); + Q_ASSERT(field); + if (field->getValue().toString() == field->getOptions().at(VehicleConfigurationHelper::MIXER_TYPE_MOTOR)) { + OutputCalibrationUtil *output = new OutputCalibrationUtil(); + output->startChannelOutput(i, LOW_PWM_OUTPUT_PULSE_LENGTH_MICROSECONDS); + output->setChannelOutputValue(HIGH_PWM_OUTPUT_PULSE_LENGTH_MICROSECONDS); + m_outputs << output; + } + } + ui->startStopButton->setText(tr("Stop")); + ui->startStopButton->setEnabled(true); + } else { + m_isCalibrating = false; + ui->startStopButton->setEnabled(false); + foreach(OutputCalibrationUtil * output, m_outputs) { + output->stopChannelOutput(); + delete output; + } + m_outputs.clear(); + m_isCalibrating = false; + ui->startStopButton->setText(tr("Start")); + ui->startStopButton->setEnabled(true); + enableButtons(true); + } +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.h new file mode 100644 index 000000000..6f4657fab --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.h @@ -0,0 +1,60 @@ +/** + ****************************************************************************** + * + * @file biascalibrationpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup BiasCalibrationPage + * @{ + * @brief + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#ifndef ESCCALIBRATIONPAGE_H +#define ESCCALIBRATIONPAGE_H + +#include "abstractwizardpage.h" +#include "outputcalibrationutil.h" + +namespace Ui { +class EscCalibrationPage; +} + +class EscCalibrationPage : public AbstractWizardPage { + Q_OBJECT + +public: + explicit EscCalibrationPage(SetupWizard *wizard, QWidget *parent = 0); + ~EscCalibrationPage(); + bool validatePage(); + +private slots: + void startStopButtonClicked(); + void enableButtons(bool enable); + +private: + const int LOW_PWM_OUTPUT_PULSE_LENGTH_MICROSECONDS = 1000; + const int HIGH_PWM_OUTPUT_PULSE_LENGTH_MICROSECONDS = 2000; + Ui::EscCalibrationPage *ui; + bool m_isCalibrating; + + QList m_outputs; +}; + +#endif // ESCCALIBRATIONPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.ui new file mode 100644 index 000000000..a431ed0cd --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/esccalibrationpage.ui @@ -0,0 +1,109 @@ + + + EscCalibrationPage + + + + 0 + 0 + 600 + 500 + + + + WizardPage + + + + + + <html><head/><body><p align="center"><span style=" font-size:12pt; font-weight:600;">OpenPilot ESC Calibration Procedure</span></p><p><span style=" font-size:10pt;">As you have selected to use a MultiRotor and Fast / Flashed ESCs, we need to calibrate the endpoints of these ESCs so they can see the full throttle range sent from the flight controller. </span></p><p><span style=" font-size:10pt;">This part of the wizard will tell you to connect the battery to your aircraft, before doing so you absolutely </span><span style=" font-size:10pt; font-weight:600; color:#f30f1d;">must remove the propellers from all motors</span><span style=" font-size:10pt;">. </span></p><p><span style=" font-size:10pt;">The steps to perform this calibration are as follows:</span></p><p><span style=" font-size:10pt;">1. Press the Start button on this page <br/>2. Connect the battery to your airframe<br/>3. Wait for ESC calibration beep(s)<br/>4. Press the Stop button on this page<br/>5. Wait for ESC confirmation beep(s)<br/>6. Disconnect battery</span></p><p><span style=" font-size:10pt;">When ready push the start button below.</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + + Qt::Vertical + + + QSizePolicy::Minimum + + + + 20 + 20 + + + + + + + + 0 + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + Start + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp index d1bd8c798..ec1aae395 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.cpp @@ -38,6 +38,7 @@ MultiPage::~MultiPage() void MultiPage::initializePage(VehicleConfigurationSource *settings) { Q_UNUSED(settings); + setSelectedItem(SetupWizard::MULTI_ROTOR_QUAD_X); } bool MultiPage::validatePage(SelectionItem *selectedItem) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui index f318aafe3..5e46b1a45 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui @@ -20,14 +20,13 @@ <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> -<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'MS Shell Dlg 2'; font-size:12pt; font-weight:600;">OpenPilot Configuration Summary</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:12pt; font-weight:600;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;">The first part of this wizard is now complete. All information required to create a basic OpenPilot controller configuration for a specific vehicle has been collected.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;">Below is a summary of the configuration and a button that links to a diagram illustrating how to connect required hardware and the OpenPilot Controller with the current configuration.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:10pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;">To continue the wizard and go through some basic configuration steps, please continue to the next step of this wizard.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;">The following steps require that your OpenPilot controller is set up according to the diagram, it is </span><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:600;">connected to the computer</span><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;"> by USB, and that the vehicle is</span><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:600;"> powered by a battery</span><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;">.</span></p></body></html> +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> +<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600;">OpenPilot Configuration Summary</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:12pt; font-weight:600;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">The first part of this wizard is now complete. All information required to create a basic OpenPilot controller configuration for a specific vehicle has been collected.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">Below is a summary of the configuration and a button that links to a diagram illustrating how to connect required hardware and the OpenPilot Controller with the current configuration.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">The following steps require that your OpenPilot controller is connected according to the diagram, remians connected to the computer by USB, and that you have a battery ready but </span><span style=" font-size:10pt; font-weight:600;">do not</span><span style=" font-size:10pt;"> connect it right now, you will be told when to in later steps of this wizard.</span></p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 749ccc179..29ec8bae3 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -40,6 +40,7 @@ #include "pages/escpage.h" #include "pages/servopage.h" #include "pages/biascalibrationpage.h" +#include "pages/esccalibrationpage.h" #include "pages/summarypage.h" #include "pages/savepage.h" #include "pages/notyetimplementedpage.h" @@ -167,6 +168,13 @@ int SetupWizard::nextId() const } case PAGE_BIAS_CALIBRATION: + if (getVehicleType() == VEHICLE_MULTI && getEscType() == ESC_RAPID) { + return PAGE_ESC_CALIBRATION; + } else { + return PAGE_OUTPUT_CALIBRATION; + } + + case PAGE_ESC_CALIBRATION: return PAGE_OUTPUT_CALIBRATION; case PAGE_OUTPUT_CALIBRATION: @@ -441,6 +449,7 @@ void SetupWizard::createPages() setPage(PAGE_ESC, new EscPage(this)); setPage(PAGE_SERVO, new ServoPage(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); + setPage(PAGE_ESC_CALIBRATION, new EscCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); setPage(PAGE_SUMMARY, new SummaryPage(this)); setPage(PAGE_SAVE, new SavePage(this)); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index 894710ab9..3a641d54d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -186,7 +186,7 @@ private slots: private: enum { PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, PAGE_AIRSPEED, PAGE_GPS, PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_ESC, PAGE_SERVO, - PAGE_BIAS_CALIBRATION, PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, + PAGE_BIAS_CALIBRATION, PAGE_ESC_CALIBRATION, PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, PAGE_NOTYETIMPLEMENTED, PAGE_AIRFRAME_INITIAL_TUNING, PAGE_REBOOT, PAGE_END, PAGE_UPDATE }; void createPages(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro index d6990971b..6dee3e178 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -36,6 +36,7 @@ HEADERS += setupwizardplugin.h \ pages/autoupdatepage.h \ pages/revocalibrationpage.h \ biascalibrationutil.h \ + pages/esccalibrationpage.h \ pages/biascalibrationpage.h \ pages/escpage.h \ pages/servopage.h \ @@ -70,6 +71,7 @@ SOURCES += setupwizardplugin.cpp \ pages/revocalibrationpage.cpp \ biascalibrationutil.cpp \ pages/biascalibrationpage.cpp \ + pages/esccalibrationpage.cpp \ pages/escpage.cpp \ pages/servopage.cpp \ pages/selectionpage.cpp \ @@ -95,6 +97,7 @@ FORMS += \ pages/autoupdatepage.ui \ pages/revocalibrationpage.ui \ pages/biascalibrationpage.ui \ + pages/esccalibrationpage.ui \ pages/escpage.ui \ pages/servopage.ui \ pages/selectionpage.ui \ diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 08c1bfa27..1e103b9d5 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -44,11 +44,6 @@ #include #include -const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; -const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 500; -const qint16 VehicleConfigurationHelper::ANALOG_SERVO_FREQUENCE = 50; -const qint16 VehicleConfigurationHelper::DIGITAL_SERVO_FREQUENCE = 333; - VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) : m_configSource(configSource), m_uavoManager(0), m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1), @@ -338,26 +333,26 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() { ActuatorSettings *actSettings = ActuatorSettings::GetInstance(m_uavoManager); - qint16 escFrequence = LEGACY_ESC_FREQUENCE; + qint16 escFrequence = LEGACY_ESC_FREQUENCY; switch (m_configSource->getEscType()) { case VehicleConfigurationSource::ESC_STANDARD: - escFrequence = LEGACY_ESC_FREQUENCE; + escFrequence = LEGACY_ESC_FREQUENCY; break; case VehicleConfigurationSource::ESC_RAPID: - escFrequence = RAPID_ESC_FREQUENCE; + escFrequence = RAPID_ESC_FREQUENCY; break; default: break; } - qint16 servoFrequence = ANALOG_SERVO_FREQUENCE; + qint16 servoFrequence = ANALOG_SERVO_FREQUENCY; switch (m_configSource->getServoType()) { case VehicleConfigurationSource::SERVO_ANALOG: - servoFrequence = ANALOG_SERVO_FREQUENCE; + servoFrequence = ANALOG_SERVO_FREQUENCY; break; case VehicleConfigurationSource::SERVO_DIGITAL: - servoFrequence = DIGITAL_SERVO_FREQUENCE; + servoFrequence = DIGITAL_SERVO_FREQUENCY; break; default: break; @@ -380,7 +375,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE; for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { - data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE; + data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCY; } switch (m_configSource->getVehicleSubType()) { @@ -597,7 +592,7 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch // Set Mixer types and values QString mixerTypePattern = "Mixer%1Type"; QString mixerVectorPattern = "Mixer%1Vector"; - for (int i = 0; i < 10; i++) { + for (int i = 0; i < ActuatorSettings::CHANNELADDR_NUMELEM; i++) { UAVObjectField *field = mSettings->getField(mixerTypePattern.arg(i + 1)); Q_ASSERT(field); field->setValue(field->getOptions().at(channels[i].type)); @@ -627,8 +622,6 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch } } - MixerSettings *mixSettings = MixerSettings::GetInstance(m_uavoManager); - // Save mixer values for sliders switch (m_configSource->getVehicleType()) { case VehicleConfigurationSource::VEHICLE_MULTI: @@ -638,35 +631,35 @@ void VehicleConfigurationHelper::applyMixerConfiguration(mixerChannelSettings ch case VehicleConfigurationSource::MULTI_ROTOR_HEXA: case VehicleConfigurationSource::MULTI_ROTOR_HEXA_H: case VehicleConfigurationSource::MULTI_ROTOR_HEXA_X: - mixSettings->setMixerValueRoll(100); - mixSettings->setMixerValuePitch(100); - mixSettings->setMixerValueYaw(100); + mSettings->setMixerValueRoll(100); + mSettings->setMixerValuePitch(100); + mSettings->setMixerValueYaw(100); break; case VehicleConfigurationSource::MULTI_ROTOR_QUAD_X: - mixSettings->setMixerValueRoll(50); - mixSettings->setMixerValuePitch(50); - mixSettings->setMixerValueYaw(50); + mSettings->setMixerValueRoll(50); + mSettings->setMixerValuePitch(50); + mSettings->setMixerValueYaw(50); break; case VehicleConfigurationSource::MULTI_ROTOR_QUAD_PLUS: - mixSettings->setMixerValueRoll(100); - mixSettings->setMixerValuePitch(100); - mixSettings->setMixerValueYaw(50); + mSettings->setMixerValueRoll(100); + mSettings->setMixerValuePitch(100); + mSettings->setMixerValueYaw(50); break; case VehicleConfigurationSource::MULTI_ROTOR_QUAD_H: - mixSettings->setMixerValueRoll(50); - mixSettings->setMixerValuePitch(70); - mixSettings->setMixerValueYaw(50); + mSettings->setMixerValueRoll(50); + mSettings->setMixerValuePitch(70); + mSettings->setMixerValueYaw(50); break; case VehicleConfigurationSource::MULTI_ROTOR_HEXA_COAX_Y: - mixSettings->setMixerValueRoll(100); - mixSettings->setMixerValuePitch(50); - mixSettings->setMixerValueYaw(66); + mSettings->setMixerValueRoll(100); + mSettings->setMixerValuePitch(50); + mSettings->setMixerValueYaw(66); break; case VehicleConfigurationSource::MULTI_ROTOR_OCTO: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_X: - mixSettings->setMixerValueRoll(100); - mixSettings->setMixerValuePitch(100); - mixSettings->setMixerValueYaw(100); + mSettings->setMixerValueRoll(100); + mSettings->setMixerValuePitch(100); + mSettings->setMixerValueYaw(100); break; case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_X: case VehicleConfigurationSource::MULTI_ROTOR_OCTO_COAX_PLUS: @@ -942,7 +935,7 @@ void VehicleConfigurationHelper::setupTriCopter() // 2. Setup GUI data // 3. Apply changes - mixerChannelSettings channels[10]; + mixerChannelSettings channels[ActuatorSettings::CHANNELADDR_NUMELEM]; GUIConfigDataUnion guiSettings = getGUIConfigData(); channels[0].type = MIXER_TYPE_MOTOR; @@ -995,7 +988,7 @@ GUIConfigDataUnion VehicleConfigurationHelper::getGUIConfigData() void VehicleConfigurationHelper::setupQuadCopter() { - mixerChannelSettings channels[10]; + mixerChannelSettings channels[ActuatorSettings::CHANNELADDR_NUMELEM]; GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame = SystemSettings::AIRFRAMETYPE_QUADX; @@ -1123,7 +1116,7 @@ void VehicleConfigurationHelper::setupQuadCopter() void VehicleConfigurationHelper::setupHexaCopter() { - mixerChannelSettings channels[10]; + mixerChannelSettings channels[ActuatorSettings::CHANNELADDR_NUMELEM]; GUIConfigDataUnion guiSettings = getGUIConfigData(); SystemSettings::AirframeTypeOptions frame = SystemSettings::AIRFRAMETYPE_HEXA; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index 750697a41..1c857ee77 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -57,20 +57,20 @@ public: VehicleConfigurationHelper(VehicleConfigurationSource *configSource); bool setupVehicle(bool save = true); bool setupHardwareSettings(bool save = true); - static const qint16 LEGACY_ESC_FREQUENCE; - static const qint16 RAPID_ESC_FREQUENCE; - static const qint16 ANALOG_SERVO_FREQUENCE; - static const qint16 DIGITAL_SERVO_FREQUENCE; + static const qint16 LEGACY_ESC_FREQUENCY = 50; + static const qint16 RAPID_ESC_FREQUENCY = 500; + static const qint16 ANALOG_SERVO_FREQUENCY = 50; + static const qint16 DIGITAL_SERVO_FREQUENCY = 333; + static const int MIXER_TYPE_NONE = 0; + static const int MIXER_TYPE_MOTOR = 1; + static const int MIXER_TYPE_REVERSABLEMOTOR = 2; + static const int MIXER_TYPE_SERVO = 3; signals: void saveProgress(int total, int current, QString description); private: - static const int MIXER_TYPE_DISABLED = 0; - static const int MIXER_TYPE_MOTOR = 1; - static const int MIXER_TYPE_REVERSABLEMOTOR = 2; - static const int MIXER_TYPE_SERVO = 3; static const float DEFAULT_ENABLED_ACCEL_TAU = 0.1; VehicleConfigurationSource *m_configSource;