From 92c19f2d7479a4fd15f4603c7924762485a3e6ce Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 24 Nov 2013 17:29:38 -0500 Subject: [PATCH 001/718] Changed the element id's in one of the SVG files that I incorrectly edited. This prevents the error: Could not resolve property : SVGID_1_ Error comes from qsvghandler.cpp or qsvgstyle.cpp in the QT libraries Qt5.2.0//5.2.0-beta1/Src/qtsvg/src/svg/ The fixedwing-shapes.svg and flyingwing-shapes.svg files need modified to prevent the errors caused by qsvgtinydocument.cpp in the QT libraries. Couldn't find node aileron. Skipping rendering. Couldn't find node vtail. Skipping rendering. These errors come from the case statement that was added to updateImageAnd Description() for FixedWingPage + switch (type) { + case SetupWizard::FIXED_WING_AILERON: + elementId = "aileron"; + break; + case SetupWizard::FIXED_WING_VTAIL: + elementId = "vtail"; + break; + default: + elementId = ""; + break; + } A similar error needs to be resolved in connectiondiagram.cpp + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::FIXED_WING_AILERON: + elementsToShow << "aileron"; + break; + case VehicleConfigurationSource::FIXED_WING_VTAIL: + elementsToShow << "vtail"; + break; + default: + break; + } Likewise outputcalibrationpage.cpp will need to reverence the elements inside the SVG file properly. + m_vehicleElementIds << "fixed-aileron" << "aileron"; + m_vehicleHighlightElementIndexes << 0 << 1; ... + m_vehicleElementIds << "fixed-vtail" << "vtail"; + m_vehicleHighlightElementIndexes << 0 << 1; Until these elements are fixed in the SVG files the wizard will not render properly and allow the user to click *next*. --- .../src/plugins/config/configgadget.qrc | 3 + .../config/images/fixedwing-shapes.svg | 4 +- .../plugins/setupwizard/connectiondiagram.cpp | 10 ++ .../setupwizard/pages/fixedwingpage.cpp | 83 ++++++++- .../plugins/setupwizard/pages/fixedwingpage.h | 17 ++ .../setupwizard/pages/fixedwingpage.ui | 155 +++++++++++++++-- .../pages/outputcalibrationpage.cpp | 17 ++ .../plugins/setupwizard/pages/vehiclepage.ui | 2 +- .../src/plugins/setupwizard/setupwizard.cpp | 17 +- .../vehicleconfigurationhelper.cpp | 157 ++++++++++++++++++ .../setupwizard/vehicleconfigurationhelper.h | 3 + .../plugins/setupwizard/wizardResources.qrc | 3 + 12 files changed, 451 insertions(+), 20 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index adffc9568..c2e2ab810 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -4,6 +4,9 @@ images/ahrs-calib.svg images/paper-plane.svg images/multirotor-shapes.svg + + images/fixedwing-shapes.svg + images/flyingwing-shapes.svg images/ccpm_setup.svg images/PipXtreme.png images/help.png diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index 63421dc38..cc85dab08 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -1,7 +1,7 @@ - - + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 4a19eb4cf..e69bf039b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -118,6 +118,16 @@ void ConnectionDiagram::setupGraphicsScene() } break; case VehicleConfigurationSource::VEHICLE_FIXEDWING: + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::FIXED_WING_AILERON: + elementsToShow << "aileron"; + break; + case VehicleConfigurationSource::FIXED_WING_VTAIL: + elementsToShow << "vtail"; + break; + default: + break; + } case VehicleConfigurationSource::VEHICLE_HELI: case VehicleConfigurationSource::VEHICLE_SURFACE: default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp index 7295cc8a3..4c1db8e67 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -27,16 +27,97 @@ #include "fixedwingpage.h" #include "ui_fixedwingpage.h" +#include "setupwizard.h" FixedWingPage::FixedWingPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), ui(new Ui::FixedWingPage) { ui->setupUi(this); - setFinalPage(true); + QSvgRenderer *renderer = new QSvgRenderer(); +// What do we do about v-tail here? + renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); + m_fixedwingPic = new QGraphicsSvgItem(); + m_fixedwingPic->setSharedRenderer(renderer); + QGraphicsScene *scene = new QGraphicsScene(this); + scene->addItem(m_fixedwingPic); + ui->typeGraphicsView->setScene(scene); + + setupFixedWingTypesCombo(); + + // Default to Aileron setup + ui->typeCombo->setCurrentIndex(0); + connect(ui->typeCombo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateImageAndDescription())); + ui->typeGraphicsView->setSceneRect(m_fixedwingPic->boundingRect()); + ui->typeGraphicsView->fitInView(m_fixedwingPic, Qt::KeepAspectRatio); + } FixedWingPage::~FixedWingPage() { delete ui; } + +void FixedWingPage::initializePage() +{ + updateAvailableTypes(); + updateImageAndDescription(); +} + +bool FixedWingPage::validatePage() +{ + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + + getWizard()->setVehicleSubType(type); + return true; +} + +void FixedWingPage::resizeEvent(QResizeEvent *event) +{ + Q_UNUSED(event); + if (m_fixedwingPic) { + ui->typeGraphicsView->setSceneRect(m_fixedwingPic->boundingRect()); + ui->typeGraphicsView->fitInView(m_fixedwingPic, Qt::KeepAspectRatio); + } +} + +void FixedWingPage::setupFixedWingTypesCombo() +{ + ui->typeCombo->addItem(tr("Aileron, Elevator, Rudder"), SetupWizard::FIXED_WING_AILERON); + m_descriptions << tr("A description for aileron driven fixed wing stuff goes here... "); + + ui->typeCombo->addItem(tr("V-Tail, or Elevon"), SetupWizard::FIXED_WING_VTAIL); + m_descriptions << tr("A description for vtail driven fixed wing stuff goes here... "); +} + +void FixedWingPage::updateAvailableTypes() +{ +} + +void FixedWingPage::updateImageAndDescription() +{ + + + SetupWizard::VEHICLE_SUB_TYPE type = (SetupWizard::VEHICLE_SUB_TYPE)ui->typeCombo->itemData(ui->typeCombo->currentIndex()).toInt(); + QString elementId = ""; + QString description = m_descriptions.at(ui->typeCombo->currentIndex()); + + switch (type) { + case SetupWizard::FIXED_WING_AILERON: + elementId = "aileron"; + break; + case SetupWizard::FIXED_WING_VTAIL: + elementId = "vtail"; + break; + default: + elementId = ""; + break; + } + m_fixedwingPic->setElementId(elementId); + ui->typeGraphicsView->setSceneRect(m_fixedwingPic->boundingRect()); + ui->typeGraphicsView->fitInView(m_fixedwingPic, Qt::KeepAspectRatio); + + ui->typeDescription->setText(description); + +} + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h index 6a99ef2fe..593be37bb 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.h @@ -28,6 +28,10 @@ #ifndef FIXEDWINGPAGE_H #define FIXEDWINGPAGE_H +#include +#include +#include + #include "abstractwizardpage.h" namespace Ui { @@ -41,8 +45,21 @@ public: explicit FixedWingPage(SetupWizard *wizard, QWidget *parent = 0); ~FixedWingPage(); + void initializePage(); + bool validatePage(); + +protected: + void resizeEvent(QResizeEvent *event); + private: Ui::FixedWingPage *ui; + void setupFixedWingTypesCombo(); + QGraphicsSvgItem *m_fixedwingPic; + void updateAvailableTypes(); + QList m_descriptions; + +private slots: + void updateImageAndDescription(); }; #endif // FIXEDWINGPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui index 8b478be7d..ad47e2685 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui @@ -15,22 +15,147 @@ - - - <!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:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">The Fixed Wing section of the OpenPilot Setup Wizard is not yet implemented</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:8pt;"></p></body></html> + + + <!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:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> +<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:12pt; font-weight:600;">OpenPilot fixedwing 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: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;">This part of the wizard will set up the OpenPilot controller for use with a flying platform utilizing multiple rotors. The wizard supports the most common types of fixedwings. Other variants of fixedwings can be configured by using custom configuration options in the Configuration plugin in the GCS.</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;">Please select the type of fixedwing you want to create a configuration for below:</span></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + + 4 - - Qt::AlignCenter - - - true - - + + + + + + 4 + + + + + + 125 + 36 + + + + + 10 + 50 + false + + + + Multirotor type: + + + + + + + + 125 + 20 + + + + + + + + + + + 0 + 0 + + + + Qt::ScrollBarAlwaysOn + + + Qt::ScrollBarAlwaysOff + + + Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + + + + + + 0 + 0 + + + + + 200 + 200 + + + + true + + + QFrame::NoFrame + + + 0 + + + 0 + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + + 0 + 0 + 0 + + + + + + + 0 + 0 + 0 + + + + + false + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index c1eebfa1d..f43304352 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -104,6 +104,23 @@ void OutputCalibrationPage::setupVehicle() m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; break; +// KF hack + case SetupWizard::FIXED_WING_AILERON: + qDebug() << "no clue what a wizard index is!"; + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "fixed-aileron" << "aileron"; + m_vehicleHighlightElementIndexes << 0 << 1; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + getWizard()->setActuatorSettings(m_actuatorSettings); + break; + case SetupWizard::FIXED_WING_VTAIL: + qDebug() << "no clue what a wizard index is!"; + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "fixed-vtail" << "vtail"; + m_vehicleHighlightElementIndexes << 0 << 1; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + getWizard()->setActuatorSettings(m_actuatorSettings); + break; default: break; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui index a6e0e9354..34d9531fe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui @@ -146,7 +146,7 @@ p, li { white-space: pre-wrap; } - false + true diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index de3b20063..03073cc6d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -225,7 +225,22 @@ QString SetupWizard::getSummaryText() break; case VEHICLE_FIXEDWING: - summary.append(tr("Fixed wing")); + summary.append(tr("Fixed wing")); + + summary.append("
"); + summary.append("").append(tr("Vehicle sub type: ")).append(""); + switch (getVehicleSubType()) { + case SetupWizard::FIXED_WING_AILERON: + summary.append(tr("Aileron")); + break; + case SetupWizard::FIXED_WING_VTAIL: + summary.append(tr("Vtail")); + break; + default: + summary.append(tr("Unknown")); + break; + } + break; case VEHICLE_HELI: summary.append(tr("Helicopter")); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 6d8e0fd7d..e07a614d9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -214,6 +214,22 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() break; } case VehicleConfigurationSource::VEHICLE_FIXEDWING: + { + switch (m_configSource->getVehicleSubType()) { + case VehicleConfigurationSource::FIXED_WING_AILERON: + setupAileron(); + break; +/* case VehicleConfigurationSource::FIXED_WING_ELEVON: + setupElevon(); + break; */ + case VehicleConfigurationSource::FIXED_WING_VTAIL: + setupVtail(); + break; + default: + break; + } + break; + } case VehicleConfigurationSource::VEHICLE_HELI: case VehicleConfigurationSource::VEHICLE_SURFACE: // TODO: Implement settings for other vehicle types? @@ -1287,3 +1303,144 @@ void VehicleConfigurationHelper::setupOctoCopter() applyMixerConfiguration(channels); applyMultiGUISettings(frame, guiSettings); } + +// This is all wrong... I just copied the Tricopter stuff +void VehicleConfigurationHelper::setupVtail() +{ + + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 50; + channels[0].yaw = 0; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 50; + channels[1].yaw = 0; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = 0; + + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 100; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.TRIYaw = 4; + + applyMixerConfiguration(channels); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL, guiSettings); +} + +// This is all wrong... I just copied the Tricopter stuff +void VehicleConfigurationHelper::setupAileron() +{ + // Typical vehicle setup + // 1. Setup mixer data + // 2. Setup GUI data + // 3. Apply changes + + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 50; + channels[0].yaw = 0; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 50; + channels[1].yaw = 0; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = 0; + + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 100; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.TRIYaw = 4; + + applyMixerConfiguration(channels); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL, guiSettings); +} + +// This is all wrong... I just copied the Tricopter stuff +void VehicleConfigurationHelper::setupElevon() +{ + // Typical vehicle setup + // 1. Setup mixer data + // 2. Setup GUI data + // 3. Apply changes + + mixerChannelSettings channels[10]; + GUIConfigDataUnion guiSettings = getGUIConfigData(); + + channels[0].type = MIXER_TYPE_MOTOR; + channels[0].throttle1 = 100; + channels[0].throttle2 = 0; + channels[0].roll = 100; + channels[0].pitch = 50; + channels[0].yaw = 0; + + channels[1].type = MIXER_TYPE_MOTOR; + channels[1].throttle1 = 100; + channels[1].throttle2 = 0; + channels[1].roll = -100; + channels[1].pitch = 50; + channels[1].yaw = 0; + + channels[2].type = MIXER_TYPE_MOTOR; + channels[2].throttle1 = 100; + channels[2].throttle2 = 0; + channels[2].roll = 0; + channels[2].pitch = -100; + channels[2].yaw = 0; + + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 100; + + guiSettings.multi.VTOLMotorNW = 1; + guiSettings.multi.VTOLMotorNE = 2; + guiSettings.multi.VTOLMotorS = 3; + guiSettings.multi.TRIYaw = 4; + + applyMixerConfiguration(channels); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON, guiSettings); +} + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index 1ed1aa576..197f1a435 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -103,6 +103,9 @@ private: void setupQuadCopter(); void setupHexaCopter(); void setupOctoCopter(); + void setupVtail(); + void setupAileron(); + void setupElevon(); private slots: void uAVOTransactionCompleted(UAVObject *object, bool success); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc index 1ee031d0e..2c1c13e3c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc +++ b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc @@ -30,6 +30,9 @@ resources/bttn-turbo-down.png resources/bttn-turbo-up.png resources/multirotor-shapes.svg + + resources/fixedwing-shapes.svg + resources/flyingwing-shapes.svg resources/bttn-illustration-down.png resources/bttn-illustration-up.png resources/bttn-save-down.png From 0d5e91ee266bf3f75ebae4b501fe1ce5d488c8b4 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Mon, 25 Nov 2013 01:06:07 -0500 Subject: [PATCH 002/718] I fixed the first screen, now the vtail and aileron models show when slected. For some reason I am unable to move beyond the first wizard screen. --- .../src/plugins/config/configgadget.qrc | 1 - .../config/images/fixedwing-shapes.svg | 3625 +++++++++++++---- .../config/images/flyingwing-shapes.svg | 544 --- .../setupwizard/pages/fixedwingpage.cpp | 1 - .../setupwizard/pages/fixedwingpage.ui | 2 +- .../pages/outputcalibrationpage.cpp | 7 +- .../resources/fixedwing-shapes.svg | 3625 +++++++++++++---- .../resources/flyingwing-shapes.svg | 544 --- .../plugins/setupwizard/wizardResources.qrc | 1 - 9 files changed, 5584 insertions(+), 2766 deletions(-) delete mode 100644 ground/openpilotgcs/src/plugins/config/images/flyingwing-shapes.svg delete mode 100644 ground/openpilotgcs/src/plugins/setupwizard/resources/flyingwing-shapes.svg diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index c2e2ab810..f58ce7203 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -6,7 +6,6 @@ images/multirotor-shapes.svg images/fixedwing-shapes.svg - images/flyingwing-shapes.svg images/ccpm_setup.svg images/PipXtreme.png images/help.png diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index cc85dab08..e99a33c65 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -1,836 +1,2789 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + +image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/images/flyingwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/flyingwing-shapes.svg deleted file mode 100644 index 62ecd3aaf..000000000 --- a/ground/openpilotgcs/src/plugins/config/images/flyingwing-shapes.svg +++ /dev/null @@ -1,544 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp index 4c1db8e67..543da261e 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -35,7 +35,6 @@ FixedWingPage::FixedWingPage(SetupWizard *wizard, QWidget *parent) : { ui->setupUi(this); QSvgRenderer *renderer = new QSvgRenderer(); -// What do we do about v-tail here? renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); m_fixedwingPic = new QGraphicsSvgItem(); m_fixedwingPic->setSharedRenderer(renderer); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui index ad47e2685..b1c5f253f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui @@ -63,7 +63,7 @@ - Multirotor type: + FixedWing type: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index f43304352..a6e5d74f9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -36,6 +36,7 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren { ui->setupUi(this); + qDebug() << "calling output calibration page"; m_vehicleRenderer = new QSvgRenderer(); if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && @@ -107,7 +108,8 @@ void OutputCalibrationPage::setupVehicle() // KF hack case SetupWizard::FIXED_WING_AILERON: qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; +// m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; m_vehicleElementIds << "fixed-aileron" << "aileron"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; @@ -115,7 +117,8 @@ void OutputCalibrationPage::setupVehicle() break; case SetupWizard::FIXED_WING_VTAIL: qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; +// m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg index 63421dc38..e99a33c65 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg @@ -1,836 +1,2789 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + +image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/flyingwing-shapes.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/flyingwing-shapes.svg deleted file mode 100644 index 62ecd3aaf..000000000 --- a/ground/openpilotgcs/src/plugins/setupwizard/resources/flyingwing-shapes.svg +++ /dev/null @@ -1,544 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc index 2c1c13e3c..507b4c877 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc +++ b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc @@ -32,7 +32,6 @@ resources/multirotor-shapes.svg resources/fixedwing-shapes.svg - resources/flyingwing-shapes.svg resources/bttn-illustration-down.png resources/bttn-illustration-up.png resources/bttn-save-down.png From 2b585a861878ef034c236966d6605f55b08836cf Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sat, 14 Dec 2013 16:46:26 -0500 Subject: [PATCH 003/718] added a case statement for PAGE_FIXEDWING to trigger PAGE_OUTPUT --- ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 03073cc6d..449296f5b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -112,6 +112,9 @@ int SetupWizard::nextId() const case PAGE_MULTI: return PAGE_OUTPUT; + case PAGE_FIXEDWING: + return PAGE_OUTPUT; + case PAGE_INPUT: if (isRestartNeeded()) { saveHardwareSettings(); From a042ffd843338d69270b698482132b16b6f80784 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 15 Dec 2013 03:38:12 -0500 Subject: [PATCH 004/718] just trying to cleanup a bad push from last night. I stomped on some things I should not have. --- .../openpilotgcs/src/plugins/config/airframe_fixedwing.ui | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/airframe_fixedwing.ui b/ground/openpilotgcs/src/plugins/config/airframe_fixedwing.ui index 1b42f042d..176b5daca 100644 --- a/ground/openpilotgcs/src/plugins/config/airframe_fixedwing.ui +++ b/ground/openpilotgcs/src/plugins/config/airframe_fixedwing.ui @@ -37,6 +37,14 @@ Airplane type: + + + + 0 + 0 + + + From 632ffed7079e9528aaa435069ac5cb504b996449 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 15 Dec 2013 21:37:19 -0500 Subject: [PATCH 005/718] Add the fixed wing plane art to the channel config screen (in progress) Begin adding plane art to Output config screen in wizard. --- .../configfixedwingwidget.cpp | 99 +++++++++++++++++-- .../cfg_vehicletypes/configfixedwingwidget.h | 15 ++- .../pages/outputcalibrationpage.cpp | 3 + 3 files changed, 104 insertions(+), 13 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 1a667fa5e..57a204a10 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -40,6 +40,8 @@ #include #include +const QString ConfigFixedWingWidget::CHANNELBOXNAME = QString("fixedWingChannelBox"); + QStringList ConfigFixedWingWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults @@ -83,6 +85,32 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : populateChannelComboBoxes(); + QSvgRenderer *renderer = new QSvgRenderer(); + renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); + plane = new QGraphicsSvgItem(); + plane->setSharedRenderer(renderer); + + QString type = "aileron"; // This needs fixed. Need to be able to obtain the aircraft type. + + // not sure why m_aircraft->fixedWingType->currentText() is not working here! fix it + if (type == "vtail") + { + plane->setElementId("vtail"); + } + else if (type == "aileron") + { + plane->setElementId("aileron"); + } + else + { + plane->setElementId("unknown"); + } + + QGraphicsScene *scene = new QGraphicsScene(); + scene->addItem(plane); + scene->setSceneRect(plane->boundingRect()); + m_aircraft->planeShape->setScene(scene); + QStringList fixedWingTypes; fixedWingTypes << "Elevator aileron rudder" << "Elevon" << "Vtail"; m_aircraft->fixedWingType->addItems(fixedWingTypes); @@ -90,6 +118,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : // Set default model to "Elevator aileron rudder" connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); + setupUI(m_aircraft->fixedWingType->currentText()); } @@ -158,6 +187,26 @@ void ConfigFixedWingWidget::setupUI(QString frameType) } } +void ConfigFixedWingWidget::setupEnabledControls(QString frameType) +{ + + // disable all motor channel boxes + for (int i = 1; i <= 8; i++) { + // do it manually so we can turn off any error decorations + QComboBox *combobox = this->findChild("fixedWingChannelBox" + QString::number(i)); + if (combobox) { + combobox->setEnabled(false); + combobox->setItemData(0, 0, Qt::DecorationRole); + } + } + + if (frameType == "Vtail" || frameType == "Elevon") { + enableComboBoxes(this, CHANNELBOXNAME, 3, true); + } else if (frameType == "aileron" || frameType == "Elevator aileron rudder") { + enableComboBoxes(this, CHANNELBOXNAME, 4, true); + } +} + void ConfigFixedWingWidget::registerWidgets(ConfigTaskWidget &parent) { parent.addWidget(m_aircraft->fixedWingThrottle->getCurveWidget()); @@ -278,6 +327,16 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() return airframeType; } +void ConfigFixedWingWidget::updateAirframe(QString frameType) +{ + qDebug() << "ConfigFixedWingWidget::updateAirframe - frame type" << frameType; + + // this is not doing anything right now + + m_aircraft->planeShape->setSceneRect(plane->boundingRect()); + m_aircraft->planeShape->fitInView(plane, Qt::KeepAspectRatio); +} + /** Setup Elevator/Aileron/Rudder airframe. @@ -502,15 +561,6 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) return true; } -void ConfigFixedWingWidget::enableControls(bool enable) -{ - ConfigTaskWidget::enableControls(enable); - - if (enable) { - setupUI(m_aircraft->fixedWingType->currentText()); - } -} - /** This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ @@ -600,3 +650,34 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) return error; } + +/** + WHAT DOES THIS DO??? + */ +void ConfigFixedWingWidget::showEvent(QShowEvent *event) +{ + Q_UNUSED(event) + // Thit fitInView method should only be called now, once the + // widget is shown, otherwise it cannot compute its values and + // the result is usually a ahrsbargraph that is way too small. + m_aircraft->planeShape->fitInView(plane, Qt::KeepAspectRatio); +} + +/** + Resize the GUI contents when the user changes the window size + */ +void ConfigFixedWingWidget::resizeEvent(QResizeEvent *event) +{ + Q_UNUSED(event); + m_aircraft->planeShape->fitInView(plane, Qt::KeepAspectRatio); +} + +void ConfigFixedWingWidget::enableControls(bool enable) +{ + ConfigTaskWidget::enableControls(enable); + + if (enable) { + setupEnabledControls(m_aircraft->fixedWingType->currentText()); + } +} + diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 65c03c86e..da8ef3839 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -34,8 +34,9 @@ #include "uavobjectmanager.h" #include "uavobject.h" #include "uavtalk/telemetrymanager.h" + +#include #include -#include #include class Ui_Widget; @@ -44,6 +45,7 @@ class ConfigFixedWingWidget : public VehicleConfig { Q_OBJECT public: + static const QString CHANNELBOXNAME; static QStringList getChannelDescriptions(); ConfigFixedWingWidget(QWidget *parent = 0); @@ -52,8 +54,14 @@ public: virtual void refreshWidgetsValues(QString frameType); virtual QString updateConfigObjectsFromWidgets(); +protected: + void showEvent(QShowEvent *event); + void resizeEvent(QResizeEvent *event); + void enableControls(bool enable); + private: Ui_FixedWingConfigWidget *m_aircraft; + QGraphicsSvgItem *plane; virtual void registerWidgets(ConfigTaskWidget &parent); virtual void resetActuators(GUIConfigDataUnion *configData); @@ -62,9 +70,8 @@ private: bool setupFrameElevon(QString airframeType); bool setupFrameVtail(QString airframeType); -protected: - void enableControls(bool enable); - + void updateAirframe(QString multiRotorType); + void setupEnabledControls(QString airframeType); private slots: virtual void setupUI(QString airframeType); virtual bool throwConfigError(QString airframeType); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index a6e5d74f9..2741e1b7b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -38,6 +38,9 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren qDebug() << "calling output calibration page"; m_vehicleRenderer = new QSvgRenderer(); + +// need to determine multi rotor or fixed wing. and pick the right svg + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && m_vehicleRenderer->isValid()) { From 82e2c5cf924613c654a97579bfd3f483d956df37 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Mon, 16 Dec 2013 14:29:04 -0500 Subject: [PATCH 006/718] Move some code around to accomidate multiple svg images in the output calibration screen. Need to fix the SVG to allow for zooming in on specific channel components as we do with the multi rotor screen. This has some sloppy comments in if for my own debugging purposes. --- .../configfixedwingwidget.cpp | 1 + .../pages/outputcalibrationpage.cpp | 55 ++++++++++++++++--- 2 files changed, 49 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 57a204a10..af76c818d 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -92,6 +92,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : QString type = "aileron"; // This needs fixed. Need to be able to obtain the aircraft type. + qDebug() << "Current Aircraft type: " << m_aircraft->fixedWingType->currentText(); // not sure why m_aircraft->fixedWingType->currentText() is not working here! fix it if (type == "vtail") { diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 2741e1b7b..1bbd4b4cc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -39,14 +39,10 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren qDebug() << "calling output calibration page"; m_vehicleRenderer = new QSvgRenderer(); -// need to determine multi rotor or fixed wing. and pick the right svg + // move the code that was here to setupVehicle() so we can determine which image to use. + m_vehicleScene = new QGraphicsScene(this); + ui->vehicleView->setScene(m_vehicleScene); - if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && - m_vehicleRenderer->isValid()) { - m_vehicleScene = new QGraphicsScene(this); - ui->vehicleView->setScene(m_vehicleScene); - } } OutputCalibrationPage::~OutputCalibrationPage() @@ -67,8 +63,18 @@ void OutputCalibrationPage::setupVehicle() m_channelIndex.clear(); m_currentWizardIndex = 0; m_vehicleScene->clear(); + +// KF moved code from OutputCalibrationPage() here so it can be used to detect the current vehicle +// needs to be slimmed down and not repeated. + switch (getWizard()->getVehicleSubType()) { case SetupWizard::MULTI_ROTOR_TRI_Y: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } + m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 4 << 4; @@ -79,30 +85,55 @@ void OutputCalibrationPage::setupVehicle() getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::MULTI_ROTOR_QUAD_X: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } m_wizardIndexes << 0 << 1 << 1 << 1 << 1; m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; m_channelIndex << 0 << 0 << 1 << 2 << 3; break; case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } m_wizardIndexes << 0 << 1 << 1 << 1 << 1; m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; m_channelIndex << 0 << 0 << 1 << 2 << 3; break; case SetupWizard::MULTI_ROTOR_HEXA: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; break; case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; break; case SetupWizard::MULTI_ROTOR_HEXA_H: + if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; @@ -110,6 +141,11 @@ void OutputCalibrationPage::setupVehicle() break; // KF hack case SetupWizard::FIXED_WING_AILERON: + if (QFile::exists(QString(":/setupwizard/resources/fixedwing-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/fixedwing-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } qDebug() << "no clue what a wizard index is!"; // m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; @@ -119,6 +155,11 @@ void OutputCalibrationPage::setupVehicle() getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::FIXED_WING_VTAIL: + if (QFile::exists(QString(":/setupwizard/resources/fixedwing-shapes.svg")) && + m_vehicleRenderer->load(QString(":/setupwizard/resources/fixedwing-shapes.svg")) && + m_vehicleRenderer->isValid()) { + ui->vehicleView->setScene(m_vehicleScene); + } qDebug() << "no clue what a wizard index is!"; // m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; From a960fd5c852dc84eafeebe5d8cc8bd1036263c06 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Mon, 16 Dec 2013 15:18:43 -0500 Subject: [PATCH 007/718] Fix the Configuration screen to show the new fixed wing artwork by Zen_ --- .../configfixedwingwidget.cpp | 45 ++++++++----------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index af76c818d..1c26cc512 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -85,33 +85,6 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : populateChannelComboBoxes(); - QSvgRenderer *renderer = new QSvgRenderer(); - renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); - plane = new QGraphicsSvgItem(); - plane->setSharedRenderer(renderer); - - QString type = "aileron"; // This needs fixed. Need to be able to obtain the aircraft type. - - qDebug() << "Current Aircraft type: " << m_aircraft->fixedWingType->currentText(); - // not sure why m_aircraft->fixedWingType->currentText() is not working here! fix it - if (type == "vtail") - { - plane->setElementId("vtail"); - } - else if (type == "aileron") - { - plane->setElementId("aileron"); - } - else - { - plane->setElementId("unknown"); - } - - QGraphicsScene *scene = new QGraphicsScene(); - scene->addItem(plane); - scene->setSceneRect(plane->boundingRect()); - m_aircraft->planeShape->setScene(scene); - QStringList fixedWingTypes; fixedWingTypes << "Elevator aileron rudder" << "Elevon" << "Vtail"; m_aircraft->fixedWingType->addItems(fixedWingTypes); @@ -135,7 +108,17 @@ void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); + // This had to be moved from ConfigFixedWingWidget() here since m_aircraft->fixedWingType->currentText() + // did not seem to work properly to choose alternate .svg files. + QSvgRenderer *renderer = new QSvgRenderer(); + renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); + plane = new QGraphicsSvgItem(); + plane->setSharedRenderer(renderer); + + qDebug() << "Current Aircraft type: \n" << m_aircraft->fixedWingType->currentText(); + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { + plane->setElementId("aileron"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevator aileron rudder")); m_aircraft->fwRudder1ChannelBox->setEnabled(true); m_aircraft->fwRudder2ChannelBox->setEnabled(true); @@ -152,6 +135,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { + plane->setElementId("aileron"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); m_aircraft->fwAileron1Label->setText("Elevon 1"); m_aircraft->fwAileron2Label->setText("Elevon 2"); @@ -168,6 +152,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(true); m_aircraft->elevonSlider2->setEnabled(true); } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { + plane->setElementId("vtail"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail")); m_aircraft->fwRudder1ChannelBox->setEnabled(false); m_aircraft->fwRudder2ChannelBox->setEnabled(false); @@ -186,6 +171,12 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(true); m_aircraft->elevonSlider2->setEnabled(true); } + + QGraphicsScene *scene = new QGraphicsScene(); + scene->addItem(plane); + scene->setSceneRect(plane->boundingRect()); + m_aircraft->planeShape->setScene(scene); + } void ConfigFixedWingWidget::setupEnabledControls(QString frameType) From 6e5e7e4dd374c5fbe6d5e8e4242f3f5c0bea6d80 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Mon, 16 Dec 2013 20:51:43 -0500 Subject: [PATCH 008/718] Work around for a problem caused by lack of elements in the fixed wing SVG for use in the output window. --- .../config/cfg_vehicletypes/configfixedwingwidget.cpp | 2 +- .../plugins/setupwizard/pages/outputcalibrationpage.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 1c26cc512..f7df0289e 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -135,7 +135,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { - plane->setElementId("aileron"); + plane->setElementId("vtail"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); m_aircraft->fwAileron1Label->setText("Elevon 1"); m_aircraft->fwAileron2Label->setText("Elevon 2"); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 1bbd4b4cc..a2963dfac 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -147,8 +147,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; -// m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "fixed-aileron" << "aileron"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; @@ -161,8 +160,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; -// m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_wizardIndexes << 0 << 1 << 1 << 1 << 2 << 3 << 4; + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; @@ -255,7 +253,7 @@ void OutputCalibrationPage::setWizardPage() ui->servoMaxAngleSlider->setValue(m_actuatorSettings[currentChannel].channelMax); } } - setupVehicleHighlightedPart(); + // setupVehicleHighlightedPart(); // turn this off for now, need to fix fixedwing image elements } void OutputCalibrationPage::initializePage() From 9e417f5ee5909e0b9616cd1c138912543a695a63 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 18 Dec 2013 12:57:35 -0500 Subject: [PATCH 009/718] Remove some redundant "elevon" stuff for now. Will come back later when proper "elevon + rudder" artwork exists. Currently no different from vtail anyway... Enabled proper channel mixing settings inside the FixedWing wizard Output configuration screen. Attempt to set GUIConfigDataUnion to fixedwing --- .../configfixedwingwidget.cpp | 130 +----------------- .../cfg_vehicletypes/configfixedwingwidget.h | 1 - .../config/configvehicletypewidget.cpp | 7 +- .../setupwizard/pages/fixedwingpage.cpp | 6 +- .../pages/outputcalibrationpage.cpp | 37 ++++- .../vehicleconfigurationhelper.cpp | 120 +++++----------- .../setupwizard/vehicleconfigurationhelper.h | 1 - 7 files changed, 78 insertions(+), 224 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index f7df0289e..71861c2c4 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -86,7 +86,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : populateChannelComboBoxes(); QStringList fixedWingTypes; - fixedWingTypes << "Elevator aileron rudder" << "Elevon" << "Vtail"; + fixedWingTypes << "Elevator aileron rudder" << "Vtail"; m_aircraft->fixedWingType->addItems(fixedWingTypes); // Set default model to "Elevator aileron rudder" @@ -134,23 +134,6 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); - } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { - plane->setElementId("vtail"); - setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); - m_aircraft->fwAileron1Label->setText("Elevon 1"); - m_aircraft->fwAileron2Label->setText("Elevon 2"); - m_aircraft->fwElevator1ChannelBox->setEnabled(false); - m_aircraft->fwElevator2ChannelBox->setEnabled(false); - m_aircraft->fwRudder1ChannelBox->setEnabled(true); - m_aircraft->fwRudder2ChannelBox->setEnabled(true); - - m_aircraft->fwElevator1Label->setText("Elevator 1"); - m_aircraft->fwElevator2Label->setText("Elevator 2"); - m_aircraft->elevonLabel1->setText("Roll"); - m_aircraft->elevonLabel2->setText("Pitch"); - - m_aircraft->elevonSlider1->setEnabled(true); - m_aircraft->elevonSlider2->setEnabled(true); } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { plane->setElementId("vtail"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail")); @@ -192,7 +175,7 @@ void ConfigFixedWingWidget::setupEnabledControls(QString frameType) } } - if (frameType == "Vtail" || frameType == "Elevon") { + if (frameType == "Vtail" || frameType == "vtail") { enableComboBoxes(this, CHANNELBOXNAME, 3, true); } else if (frameType == "aileron" || frameType == "Elevator aileron rudder") { enableComboBoxes(this, CHANNELBOXNAME, 4, true); @@ -263,18 +246,9 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) setComboCurrentIndex(m_aircraft->fwRudder1ChannelBox, fixed.FixedWingYaw1); setComboCurrentIndex(m_aircraft->fwRudder2ChannelBox, fixed.FixedWingYaw2); - if (frameType == "FixedWingElevon") { - // If the airframe is elevon, restore the slider setting + if (frameType == "FixedWingVtail") { + // If the airframe is vtail, restore the slider setting // Find the channel number for Elevon1 (FixedWingRoll1) - int channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; - if (channel > -1) { - // If for some reason the actuators were incoherent, we might fail here, hence the check. - m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); - m_aircraft->elevonSlider2->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); - } - } else if (frameType == "FixedWingVtail") { int channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. @@ -308,10 +282,7 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder") { airframeType = "FixedWing"; setupFrameFixedWing(airframeType); - } else if (m_aircraft->fixedWingType->currentText() == "Elevon") { - airframeType = "FixedWingElevon"; - setupFrameElevon(airframeType); - } else { // "Vtail" + } else if (m_aircraft->fixedWingType->currentText() == "vtail") { airframeType = "FixedWingVtail"; setupFrameVtail(airframeType); } @@ -404,76 +375,6 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) return true; } -/** - Setup Elevon - */ -bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) -{ - // Check coherence: - // Show any config errors in GUI - if (throwConfigError(airframeType)) { - return false; - } - - GUIConfigDataUnion config = getConfigData(); - resetActuators(&config); - - config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); - config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); - config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); - config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); - - setConfigData(config); - - UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); - Q_ASSERT(mixer); - resetMotorAndServoMixers(mixer); - - // Save the curve: - // ... and compute the matrix: - // In order to make code a bit nicer, we assume: - // - Channel dropdowns start with 'None', then 0 to 7 - - // 1. Assign the servo/motor/none for each channel - - double value; - - // motor - int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); - - // rudders - channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); - - channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); - - // ailerons - channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; - if (channel > -1) { - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value); - - channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -value); - } - - m_aircraft->fwStatusLabel->setText("Mixer generated"); - return true; -} - /** Setup VTail */ @@ -592,27 +493,6 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } else if (airframeType == "FixedWingElevon") { - if (m_aircraft->fwEngineChannelBox->currentText() == "None") { - m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes - error = true; - } else { - m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes - } - - if (m_aircraft->fwAileron1ChannelBox->currentText() == "None") { - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes - error = true; - } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes - } - - if (m_aircraft->fwAileron2ChannelBox->currentText() == "None") { - m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes - error = true; - } else { - m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes - } } else if (airframeType == "FixedWingVtail") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index da8ef3839..28126fc26 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -67,7 +67,6 @@ private: virtual void resetActuators(GUIConfigDataUnion *configData); bool setupFrameFixedWing(QString airframeType); - bool setupFrameElevon(QString airframeType); bool setupFrameVtail(QString airframeType); void updateAirframe(QString multiRotorType); diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index 0d6d4ee05..2abcc7d4c 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -68,9 +68,11 @@ QStringList ConfigVehicleTypeWidget::getChannelDescriptions() QStringList channelDesc; switch (systemSettingsData.AirframeType) { case SystemSettings::AIRFRAMETYPE_FIXEDWING: + channelDesc = ConfigFixedWingWidget::getChannelDescriptions(); + break; case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON: + // do nothing for elevon support for the time being. case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: - // fixed wing channelDesc = ConfigFixedWingWidget::getChannelDescriptions(); break; case SystemSettings::AIRFRAMETYPE_HELICP: @@ -268,8 +270,7 @@ QString ConfigVehicleTypeWidget::frameCategory(QString frameType) { QString category; - if (frameType == "FixedWing" || frameType == "Elevator aileron rudder" || frameType == "FixedWingElevon" - || frameType == "Elevon" || frameType == "FixedWingVtail" || frameType == "Vtail") { + if (frameType == "FixedWing" || frameType == "Elevator aileron rudder" || frameType == "FixedWingVtail" || frameType == "Vtail") { category = "Fixed Wing"; } else if (frameType == "Tri" || frameType == "Tricopter Y" || frameType == "QuadX" || frameType == "Quad X" || frameType == "QuadP" || frameType == "Quad +" || frameType == "Hexa" || frameType == "Hexacopter" diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp index 543da261e..9dd9b9638 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -83,10 +83,10 @@ void FixedWingPage::resizeEvent(QResizeEvent *event) void FixedWingPage::setupFixedWingTypesCombo() { ui->typeCombo->addItem(tr("Aileron, Elevator, Rudder"), SetupWizard::FIXED_WING_AILERON); - m_descriptions << tr("A description for aileron driven fixed wing stuff goes here... "); + m_descriptions << tr("This setup currently expects a traditional 4 channel setup including two ailerons (not connected by Y adapter), an elevator and a rudder. "); - ui->typeCombo->addItem(tr("V-Tail, or Elevon"), SetupWizard::FIXED_WING_VTAIL); - m_descriptions << tr("A description for vtail driven fixed wing stuff goes here... "); + ui->typeCombo->addItem(tr("V-Tail"), SetupWizard::FIXED_WING_VTAIL); + m_descriptions << tr("This setup currently expects a flying-wing setup, an elevon plus rudder setup is not yet supported. Setup should include only two elevons, and should explicitly not include a rudder."); } void FixedWingPage::updateAvailableTypes() diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index a2963dfac..f39db1abe 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -147,10 +147,29 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "fixed-aileron" << "aileron"; m_vehicleHighlightElementIndexes << 0 << 1; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; + + // see Servo city for an example. 1500 usec is center on MS85mg for example. - http://www.servocity.com/html/hs-85mg__mighty_micro.html + // make sure Aileron servo one does not go to an extreme value + m_actuatorSettings[1].channelMin = 1500; + m_actuatorSettings[1].channelNeutral = 1500; + m_actuatorSettings[1].channelMax = 1500; + // make sure Aileron servo two does not go to an extreme value + m_actuatorSettings[2].channelMin = 1500; + m_actuatorSettings[2].channelNeutral = 1500; + m_actuatorSettings[2].channelMax = 1500; + // make sure Elevator servo one does not go to an extreme value + m_actuatorSettings[3].channelMin = 1500; + m_actuatorSettings[4].channelNeutral = 1500; + m_actuatorSettings[3].channelMax = 1500; + // make sure Rudder servo one does not go to an extreme value + m_actuatorSettings[4].channelMin = 1500; + m_actuatorSettings[4].channelNeutral = 1500; + m_actuatorSettings[4].channelMax = 1500; + getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::FIXED_WING_VTAIL: @@ -160,10 +179,20 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_wizardIndexes << 0 << 1 << 1 << 1; m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + m_channelIndex << 0 << 0 << 1 << 2; + + // make sure elevon servo one does not go to an extreme value + m_actuatorSettings[1].channelMin = 1500; + m_actuatorSettings[1].channelNeutral = 1500; + m_actuatorSettings[1].channelMax = 1500; + // make sure elevon servo two does not go to an extreme value + m_actuatorSettings[2].channelMin = 1500; + m_actuatorSettings[2].channelNeutral = 1500; + m_actuatorSettings[2].channelMax = 1500; + getWizard()->setActuatorSettings(m_actuatorSettings); break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index e07a614d9..d32d0146a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -219,9 +219,6 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() case VehicleConfigurationSource::FIXED_WING_AILERON: setupAileron(); break; -/* case VehicleConfigurationSource::FIXED_WING_ELEVON: - setupElevon(); - break; */ case VehicleConfigurationSource::FIXED_WING_VTAIL: setupVtail(); break; @@ -1304,7 +1301,6 @@ void VehicleConfigurationHelper::setupOctoCopter() applyMultiGUISettings(frame, guiSettings); } -// This is all wrong... I just copied the Tricopter stuff void VehicleConfigurationHelper::setupVtail() { @@ -1314,41 +1310,32 @@ void VehicleConfigurationHelper::setupVtail() channels[0].type = MIXER_TYPE_MOTOR; channels[0].throttle1 = 100; channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 50; + channels[0].roll = 0; + channels[0].pitch = 0; channels[0].yaw = 0; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; + channels[1].type = MIXER_TYPE_SERVO; + channels[1].throttle1 = 0; channels[1].throttle2 = 0; channels[1].roll = -100; channels[1].pitch = 50; channels[1].yaw = 0; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; + channels[2].type = MIXER_TYPE_SERVO; + channels[2].throttle1 = 0; channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; + channels[2].roll = 100; + channels[2].pitch = -50; channels[2].yaw = 0; - channels[3].type = MIXER_TYPE_SERVO; - channels[3].throttle1 = 0; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; - channels[3].yaw = 100; - - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorS = 3; - guiSettings.multi.TRIYaw = 4; + guiSettings.fixedwing.FixedWingThrottle = 1; + guiSettings.fixedwing.FixedWingRoll1 = 2; + guiSettings.fixedwing.FixedWingRoll2 = 3; applyMixerConfiguration(channels); applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL, guiSettings); } -// This is all wrong... I just copied the Tricopter stuff void VehicleConfigurationHelper::setupAileron() { // Typical vehicle setup @@ -1362,24 +1349,31 @@ void VehicleConfigurationHelper::setupAileron() channels[0].type = MIXER_TYPE_MOTOR; channels[0].throttle1 = 100; channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 50; + channels[0].roll = 0; + channels[0].pitch = 0; channels[0].yaw = 0; - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; + channels[1].type = MIXER_TYPE_SERVO; + channels[1].throttle1 = 0; channels[1].throttle2 = 0; channels[1].roll = -100; - channels[1].pitch = 50; + channels[1].pitch = 0; channels[1].yaw = 0; - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; + channels[2].type = MIXER_TYPE_SERVO; + channels[2].throttle1 = 0; channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; + channels[2].roll = 100; + channels[2].pitch = 0; channels[2].yaw = 0; + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 100; + channels[3].yaw = 0; + channels[3].type = MIXER_TYPE_SERVO; channels[3].throttle1 = 0; channels[3].throttle2 = 0; @@ -1387,60 +1381,12 @@ void VehicleConfigurationHelper::setupAileron() channels[3].pitch = 0; channels[3].yaw = 100; - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorS = 3; - guiSettings.multi.TRIYaw = 4; + guiSettings.fixedwing.FixedWingThrottle = 1; + guiSettings.fixedwing.FixedWingRoll1 = 2; + guiSettings.fixedwing.FixedWingRoll2 = 3; + guiSettings.fixedwing.FixedWingPitch1 = 4; + guiSettings.fixedwing.FixedWingYaw1 = 5; applyMixerConfiguration(channels); - applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL, guiSettings); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGAILERON, guiSettings); } - -// This is all wrong... I just copied the Tricopter stuff -void VehicleConfigurationHelper::setupElevon() -{ - // Typical vehicle setup - // 1. Setup mixer data - // 2. Setup GUI data - // 3. Apply changes - - mixerChannelSettings channels[10]; - GUIConfigDataUnion guiSettings = getGUIConfigData(); - - channels[0].type = MIXER_TYPE_MOTOR; - channels[0].throttle1 = 100; - channels[0].throttle2 = 0; - channels[0].roll = 100; - channels[0].pitch = 50; - channels[0].yaw = 0; - - channels[1].type = MIXER_TYPE_MOTOR; - channels[1].throttle1 = 100; - channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 50; - channels[1].yaw = 0; - - channels[2].type = MIXER_TYPE_MOTOR; - channels[2].throttle1 = 100; - channels[2].throttle2 = 0; - channels[2].roll = 0; - channels[2].pitch = -100; - channels[2].yaw = 0; - - channels[3].type = MIXER_TYPE_SERVO; - channels[3].throttle1 = 0; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; - channels[3].yaw = 100; - - guiSettings.multi.VTOLMotorNW = 1; - guiSettings.multi.VTOLMotorNE = 2; - guiSettings.multi.VTOLMotorS = 3; - guiSettings.multi.TRIYaw = 4; - - applyMixerConfiguration(channels); - applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON, guiSettings); -} - diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index 197f1a435..d3ef88df4 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -105,7 +105,6 @@ private: void setupOctoCopter(); void setupVtail(); void setupAileron(); - void setupElevon(); private slots: void uAVOTransactionCompleted(UAVObject *object, bool success); From b6f0e5e3058e65f937f9f89f8e4dade428f76f4a Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 18 Dec 2013 15:14:56 -0500 Subject: [PATCH 010/718] Sort out issues with servo vs. motor sliders... finally figure out what wizardIndexes are! per OutputCalibrationPage::setWizardPage() --- .../pages/outputcalibrationpage.cpp | 4 ++-- .../setupwizard/vehicleconfigurationhelper.cpp | 18 +++++++++++++++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index f39db1abe..d720f6118 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -147,7 +147,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1; + m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; // These come from OutputCalibrationPage::setWizardPage() m_vehicleElementIds << "fixed-aileron" << "aileron"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; @@ -179,7 +179,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 1 << 1; + m_wizardIndexes << 0 << 2 << 2 << 2; // These come from OutputCalibrationPage::setWizardPage() m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index d32d0146a..5de6ae900 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -306,7 +306,23 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() addModifiedObject(actSettings, tr("Writing actuator settings")); break; } +/* case VehicleConfigurationSource::VEHICLE_FIXEDWING: + { + ActuatorSettings::DataFields data = actSettings->getData(); + + qDebug() << "Override center pulse for fixed wing servos\n"; + // move all but first chan to 1500 center pluse + QList actuatorSettings = m_configSource->getActuatorSettings(); + for (quint16 i = 1; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { + data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; + data.ChannelAddr[i] = i; + data.ChannelMin[i] = 1500; + data.ChannelNeutral[i] = 1500; + data.ChannelMax[i] = 1500; + } + } +*/ case VehicleConfigurationSource::VEHICLE_HELI: case VehicleConfigurationSource::VEHICLE_SURFACE: // TODO: Implement settings for other vehicle types? @@ -1388,5 +1404,5 @@ void VehicleConfigurationHelper::setupAileron() guiSettings.fixedwing.FixedWingYaw1 = 5; applyMixerConfiguration(channels); - applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGAILERON, guiSettings); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWING, guiSettings); } From 33d963d24e42964e83a93a651f291869296c1bc5 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 18 Dec 2013 16:41:38 -0500 Subject: [PATCH 011/718] prepping to fix refreshWidgetsValues() so that Output & Vehicle screens populate properly after wizard is run. --- .../configfixedwingwidget.cpp | 29 ++++++++++--------- .../pages/outputcalibrationpage.cpp | 4 +-- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 71861c2c4..72eaf436c 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -53,27 +53,28 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() // get the gui config data GUIConfigDataUnion configData = getConfigData(); + fixedGUISettingsStruct fixed = configData.fixedwing; - if (configData.fixedwing.FixedWingPitch1 > 0) { - channelDesc[configData.fixedwing.FixedWingPitch1 - 1] = QString("FixedWingPitch1"); + if (fixed.FixedWingThrottle > 0 && fixed.FixedWingThrottle <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingThrottle - 1] = QString("fixed.WingThrottle"); } - if (configData.fixedwing.FixedWingPitch2 > 0) { - channelDesc[configData.fixedwing.FixedWingPitch2 - 1] = QString("FixedWingPitch2"); + if (fixed.FixedWingPitch1 > 0 && fixed.FixedWingPitch1 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingPitch1 - 1] = QString("FixedWingPitch1"); } - if (configData.fixedwing.FixedWingRoll1 > 0) { - channelDesc[configData.fixedwing.FixedWingRoll1 - 1] = QString("FixedWingRoll1"); + if (fixed.FixedWingPitch2 > 0 && fixed.FixedWingPitch2 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingPitch2 - 1] = QString("FixedWingPitch2"); } - if (configData.fixedwing.FixedWingRoll2 > 0) { - channelDesc[configData.fixedwing.FixedWingRoll2 - 1] = QString("FixedWingRoll2"); + if (fixed.FixedWingRoll1 > 0 && fixed.FixedWingRoll1 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingRoll1 - 1] = QString("FixedWingRoll1"); } - if (configData.fixedwing.FixedWingYaw1 > 0) { - channelDesc[configData.fixedwing.FixedWingYaw1 - 1] = QString("FixedWingYaw1"); + if (fixed.FixedWingRoll2 > 0 && fixed.FixedWingRoll2 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingRoll2 - 1] = QString("FixedWingRoll2"); } - if (configData.fixedwing.FixedWingYaw2 > 0) { - channelDesc[configData.fixedwing.FixedWingYaw2 - 1] = QString("FixedWingYaw2"); + if (fixed.FixedWingYaw1 > 0 && fixed.FixedWingYaw1 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingYaw1 - 1] = QString("FixedWingYaw1"); } - if (configData.fixedwing.FixedWingThrottle > 0) { - channelDesc[configData.fixedwing.FixedWingThrottle - 1] = QString("FixedWingThrottle"); + if (fixed.FixedWingYaw2 > 0 && fixed.FixedWingYaw2 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { + channelDesc[fixed.FixedWingYaw2 - 1] = QString("FixedWingYaw2"); } return channelDesc; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index d720f6118..edb4419c9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -147,7 +147,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; // These come from OutputCalibrationPage::setWizardPage() + m_wizardIndexes << 0 << 1 << 3 << 3 << 3 << 3; // These come from OutputCalibrationPage::setWizardPage() m_vehicleElementIds << "fixed-aileron" << "aileron"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; @@ -179,7 +179,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 2 << 2 << 2; // These come from OutputCalibrationPage::setWizardPage() + m_wizardIndexes << 0 << 0 << 3 << 3; // These come from OutputCalibrationPage::setWizardPage() m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2; From de9a30e4242e7656bdc23b48adb66eddd32987b2 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 18 Dec 2013 17:16:12 -0500 Subject: [PATCH 012/718] prepping to fix refreshWidgetValues(0 all the setComboCurrentIndex() calls are missing. --- .../cfg_vehicletypes/configfixedwingwidget.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 72eaf436c..24f93339a 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -56,7 +56,7 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() fixedGUISettingsStruct fixed = configData.fixedwing; if (fixed.FixedWingThrottle > 0 && fixed.FixedWingThrottle <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { - channelDesc[fixed.FixedWingThrottle - 1] = QString("fixed.WingThrottle"); + channelDesc[fixed.FixedWingThrottle - 1] = QString("WingThrottle"); } if (fixed.FixedWingPitch1 > 0 && fixed.FixedWingPitch1 <= ConfigFixedWingWidget::CHANNEL_NUMELEM) { channelDesc[fixed.FixedWingPitch1 - 1] = QString("FixedWingPitch1"); @@ -91,10 +91,13 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : m_aircraft->fixedWingType->addItems(fixedWingTypes); // Set default model to "Elevator aileron rudder" - connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); - setupUI(m_aircraft->fixedWingType->currentText()); + // setupUI(m_aircraft->fixedWingType->currentText()); + + connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); + + updateEnableControls(); } ConfigFixedWingWidget::~ConfigFixedWingWidget() @@ -108,9 +111,12 @@ ConfigFixedWingWidget::~ConfigFixedWingWidget() void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); + Q_ASSERT(plane); // This had to be moved from ConfigFixedWingWidget() here since m_aircraft->fixedWingType->currentText() // did not seem to work properly to choose alternate .svg files. + m_aircraft->planeShape->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + m_aircraft->planeShape->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/fixedwing-shapes.svg")); plane = new QGraphicsSvgItem(); @@ -161,6 +167,9 @@ void ConfigFixedWingWidget::setupUI(QString frameType) scene->setSceneRect(plane->boundingRect()); m_aircraft->planeShape->setScene(scene); + setupEnabledControls(frameType); + // Draw the appropriate airframe + updateAirframe(frameType); } void ConfigFixedWingWidget::setupEnabledControls(QString frameType) From 6ee5baa1467452f5980b2defb11b27dc6b41e283 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 18 Dec 2013 17:28:25 -0500 Subject: [PATCH 013/718] I was aparantly missing a call to updateAirframe(frameType); --- .../cfg_vehicletypes/configfixedwingwidget.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 24f93339a..a621d1d8a 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -268,6 +268,8 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } } + + updateAirframe(frameType); } /** @@ -275,20 +277,15 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) */ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() { - QString airframeType = "FixedWing"; - - // Save the curve (common to all Fixed wing frames) UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); Q_ASSERT(mixer); - // Remove Feed Forward, it is pointless on a plane: - setMixerValue(mixer, "FeedForward", 0.0); - // Set the throttle curve setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); - // All airframe types must start with "FixedWing" + QString airframeType; + if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder") { airframeType = "FixedWing"; setupFrameFixedWing(airframeType); @@ -296,6 +293,9 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() airframeType = "FixedWingVtail"; setupFrameVtail(airframeType); } + + // Remove Feed Forward, it is pointless on a plane: + setMixerValue(mixer, "FeedForward", 0.0); return airframeType; } From 3862c9cfaa04849479fd3cca250a062694a0fe40 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Fri, 20 Dec 2013 02:08:08 -0500 Subject: [PATCH 014/718] settings still not saving to the board... trying to sort it out. Making sure all necessary codepaths exist in the FixedWing clone of the MultiRotor code for the Wizard. --- .../configfixedwingwidget.cpp | 114 ++++++++++++++++-- .../cfg_vehicletypes/configfixedwingwidget.h | 2 + .../pages/outputcalibrationpage.cpp | 2 +- 3 files changed, 110 insertions(+), 8 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index a621d1d8a..07ba0a4ba 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -96,7 +97,6 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : // setupUI(m_aircraft->fixedWingType->currentText()); connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); - updateEnableControls(); } @@ -105,9 +105,6 @@ ConfigFixedWingWidget::~ConfigFixedWingWidget() delete m_aircraft; } -/** - Virtual function to setup the UI - */ void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); @@ -273,7 +270,7 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) } /** - Virtual function to update the UI widget objects + Helper function to update the UI widget objects */ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() { @@ -281,17 +278,51 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() Q_ASSERT(mixer); - // Set the throttle curve + // Curve is also common to all quads: setThrottleCurve(mixer, VehicleConfig::MIXER_THROTTLECURVE1, m_aircraft->fixedWingThrottle->getCurve()); QString airframeType; + QList motor_servo_List; if (m_aircraft->fixedWingType->currentText() == "Elevator aileron rudder") { airframeType = "FixedWing"; setupFrameFixedWing(airframeType); - } else if (m_aircraft->fixedWingType->currentText() == "vtail") { + + motor_servo_List << "FixedWingThrottle" << "FixedWingPitch1" << "FixedWingPitch2" << "FixedWingRoll1" << "FixedWingRoll2" << "FixedWingYaw1" << "FixedWingYaw2"; + setupMotors(motor_servo_List); + + GUIConfigDataUnion config = getConfigData(); + setConfigData(config); + + m_aircraft->fwStatusLabel->setText(tr("Configuration OK")); + + } + else if (m_aircraft->fixedWingType->currentText() == "vtail") { airframeType = "FixedWingVtail"; setupFrameVtail(airframeType); + + motor_servo_List << "FixedWingThrottle" << "FixedWingRoll1" << "FixedWingRoll2"; + setupMotors(motor_servo_List); + + GUIConfigDataUnion config = getConfigData(); + setConfigData(config); + + // Vtail Layout: + // pitch roll yaw + double mixerMatrix[8][3] = { + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 }, + { 0, 0, 0 } + }; + setupFixedWingMixer(mixerMatrix); + + m_aircraft->fwStatusLabel->setText(tr("Configuration OK")); + } // Remove Feed Forward, it is pointless on a plane: @@ -300,6 +331,39 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() return airframeType; } +void ConfigFixedWingWidget::setupMotors(QList motorList) +{ + QList mmList; + mmList << m_aircraft->fwEngineChannelBox << m_aircraft->fwAileron1ChannelBox + << m_aircraft->fwAileron2ChannelBox << m_aircraft->fwElevator1ChannelBox + << m_aircraft->fwElevator2ChannelBox << m_aircraft->fwRudder1ChannelBox + << m_aircraft->fwRudder2ChannelBox; + + GUIConfigDataUnion configData = getConfigData(); + resetActuators(&configData); + + foreach(QString motor, motorList) { + int index = mmList.takeFirst()->currentIndex(); + + if (motor == QString("FixedWingThrottle")) { + configData.fixedwing.FixedWingThrottle = index; + } else if (motor == QString("FixedWingPitch1")) { + configData.fixedwing.FixedWingPitch1 = index; + } else if (motor == QString("FixedWingPitch2")) { + configData.fixedwing.FixedWingPitch2 = index; + } else if (motor == QString("FixedWingRoll1")) { + configData.fixedwing.FixedWingRoll1 = index; + } else if (motor == QString("FixedWingRoll2")) { + configData.fixedwing.FixedWingRoll2 = index; + } else if (motor == QString("FixedWingYaw1")) { + configData.fixedwing.FixedWingYaw1 = index; + } else if (motor == QString("FixedWingYaw2")) { + configData.fixedwing.FixedWingYaw1 = index; + } + } + setConfigData(configData); +} + void ConfigFixedWingWidget::updateAirframe(QString frameType) { qDebug() << "ConfigFixedWingWidget::updateAirframe - frame type" << frameType; @@ -334,6 +398,7 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); config.fixedwing.FixedWingYaw1 = m_aircraft->fwRudder1ChannelBox->currentIndex(); + config.fixedwing.FixedWingYaw2 = m_aircraft->fwRudder2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); setConfigData(config); @@ -464,6 +529,41 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) return true; } +/** + This function sets up the vtail fixed wing mixer values. + */ +bool ConfigFixedWingWidget::setupFixedWingMixer(double mixerFactors[8][3]) +{ + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + + Q_ASSERT(mixer); + resetMotorAndServoMixers(mixer); + + // and enable only the relevant channels: +// double pFactor = (double)m_aircraft->fwPitchMixLevel->value() / 100.0; +// double rFactor = (double)m_aircraft->fwRollMixLevel->value() / 100.0; + //invertMotors = m_aircraft->MultirotorRevMixerCheckBox->isChecked(); +// double yFactor = (double)m_aircraft->fwYawMixLevel->value() / 100.0; + + QList mmList; + mmList << m_aircraft->fwEngineChannelBox << m_aircraft->fwAileron1ChannelBox + << m_aircraft->fwAileron2ChannelBox << m_aircraft->fwElevator1ChannelBox + << m_aircraft->fwElevator2ChannelBox << m_aircraft->fwRudder1ChannelBox + << m_aircraft->fwRudder2ChannelBox; + + for (int i = 0; i < 8; i++) { + if (mmList.at(i)->isEnabled()) { + int channel = mmList.at(i)->currentIndex() - 1; + if (channel > -1) { + qDebug() << "code needs to be written here!"; +// setupQuadMotor(channel, mixerFactors[i][0] * pFactor, rFactor * mixerFactors[i][1], +// yFactor * mixerFactors[i][2]); + } + } + } + return true; +} + /** This function displays text and color formatting in order to help the user understand what channels have not yet been configured. */ diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 28126fc26..6c6bc7bab 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -68,6 +68,8 @@ private: bool setupFrameFixedWing(QString airframeType); bool setupFrameVtail(QString airframeType); + bool setupFixedWingMixer(double mixerFactors[8][3]); + void setupMotors(QList motorList); void updateAirframe(QString multiRotorType); void setupEnabledControls(QString airframeType); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index edb4419c9..c469fa2cc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -179,7 +179,7 @@ void OutputCalibrationPage::setupVehicle() ui->vehicleView->setScene(m_vehicleScene); } qDebug() << "no clue what a wizard index is!"; - m_wizardIndexes << 0 << 0 << 3 << 3; // These come from OutputCalibrationPage::setWizardPage() + m_wizardIndexes << 0 << 1 << 3 << 3; // These come from OutputCalibrationPage::setWizardPage() m_vehicleElementIds << "fixed-vtail" << "vtail"; m_vehicleHighlightElementIndexes << 0 << 1; m_channelIndex << 0 << 0 << 1 << 2; From a99de5d32661d450cecb14dc5caddbda5d167ff2 Mon Sep 17 00:00:00 2001 From: m_thread Date: Fri, 14 Feb 2014 00:00:52 +0100 Subject: [PATCH 015/718] OP-1222 Fixed svg files. Cleaned up and grouped items to work in wizard. Removed some translations and simplified some gradients. Added support for output calibration of Aileron and v-tail wings. Added comments in the code explaining the various arrays with indexes and ids in the output configuration page class. --- .../config/images/fixedwing-shapes.svg | 4294 +++++-------- .../setupwizard/pages/fixedwingpage.cpp | 2 +- .../pages/outputcalibrationpage.cpp | 100 +- .../setupwizard/pages/outputcalibrationpage.h | 4 + .../pages/outputcalibrationpage.ui | 6 + .../resources/fixedwing-shapes.svg | 5298 ++++++++--------- 6 files changed, 4114 insertions(+), 5590 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index e99a33c65..26045315f 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -19,2771 +19,1531 @@ viewBox="0 0 792 2016" enable-background="new 0 0 792 1008" xml:space="preserve" - inkscape:version="0.48.2 r9819" + inkscape:version="0.48.4 r9939" sodipodi:docname="fixedwing-shapes.svg">image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + id="metadata4103">image/svg+xml \ No newline at end of file From 02e62f91edf8326fce00ff9a1f7949a630441ec5 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Sat, 10 May 2014 14:50:24 +0200 Subject: [PATCH 016/718] OP-1317 Apadted airspeed calculation from ground speed and orientattion changes: applied extended IMU wind estimation, filtering takes place now with Holt-Winters double exponential smoothing on orientation vector and speed vector, matrix calculation replaced by a direct calculation of the fuselage vector, renamed gps_airspeed into imu_airspeed because estimation relies on imu results and a n estimate of ground speed and not directly a GPS signal --- flight/libraries/CoordinateConversions.c | 54 ++++- flight/libraries/inc/CoordinateConversions.h | 12 ++ flight/modules/Airspeed/airspeed.c | 6 +- flight/modules/Airspeed/gps_airspeed.c | 185 ------------------ flight/modules/Airspeed/inc/gps_airspeed.h | 42 ---- shared/uavobjectdefinition/airspeedsensor.xml | 6 + 6 files changed, 74 insertions(+), 231 deletions(-) delete mode 100644 flight/modules/Airspeed/gps_airspeed.c delete mode 100644 flight/modules/Airspeed/inc/gps_airspeed.h diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index fc4c66276..c3a4f2382 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -180,7 +180,7 @@ void RPY2Quaternion(const float rpy[3], float q[4]) // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]) { - float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; + const float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3]; Rbe[0][0] = q0s + q1s - q2s - q3s; Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]); @@ -193,6 +193,58 @@ void Quaternion2R(float q[4], float Rbe[3][3]) Rbe[2][2] = q0s - q1s - q2s + q3s; } + +// ** Find x of body frame from quaternion ** +void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + x[0] = q0s + q1s - q2s - q3s; + x[1] = 2 * (q1 * q2 + q0 * q3); + x[2] = 2 * (q1 * q3 - q0 * q2); +} + + +void Quaternion2xB(const float q[4], float x[3]) +{ + QuaternionC2xB(q[0], q[1], q[2], q[3], x); +} + + +// ** Find y of body frame from quaternion ** +void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + y[0] = 2 * (q1 * q2 - q0 * q3); + y[1] = q0s - q1s + q2s - q3s; + y[2] = 2 * (q2 * q3 + q0 * q1); +} + + +void Quaternion2yB(const float q[4], float y[3]) +{ + QuaternionC2yB(q[0], q[1], q[2], q[3], y); +} + + +// ** Find z of body frame from quaternion ** +void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]) +{ + const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; + + z[0] = 2 * (q1 * q3 + q0 * q2); + z[1] = 2 * (q2 * q3 - q0 * q1); + z[2] = q0s - q1s - q2s + q3s; +} + + +void Quaternion2zB(const float q[4], float z[3]) +{ + QuaternionC2zB(q[0], q[1], q[2], q[3], z); +} + + // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index 8c5f55858..a2136c73d 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -50,6 +50,18 @@ void RPY2Quaternion(const float rpy[3], float q[4]); // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]); +// ** Find x of body frame from quaternion ** +void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]); +void Quaternion2xB(const float q[4], float x[3]); + +// ** Find y of body frame from quaternion ** +void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]); +void Quaternion2yB(const float q[4], float y[3]); + +// ** Find x of body frame from quaternion ** +void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]); +void Quaternion2zB(const float q[4], float z[3]); + // ****** Express LLA in a local NED Base Frame ******** void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]); diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 8f368bef7..456131a90 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -44,7 +44,7 @@ #include "baro_airspeed_ms4525do.h" #include "baro_airspeed_etasv3.h" #include "baro_airspeed_mpxv.h" -#include "gps_airspeed.h" +#include "imu_airspeed.h" #include "taskinfo.h" // Private constants @@ -143,7 +143,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) AirspeedSettingsUpdatedCb(NULL); - gps_airspeedInitialize(); + imu_airspeedInitialize(); airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; @@ -183,7 +183,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) break; #endif case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: - gps_airspeedGet(&airspeedData, &airspeedSettings); + imu_airspeedGet(&airspeedData, &airspeedSettings); break; case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE: default: diff --git a/flight/modules/Airspeed/gps_airspeed.c b/flight/modules/Airspeed/gps_airspeed.c deleted file mode 100644 index 5486f8161..000000000 --- a/flight/modules/Airspeed/gps_airspeed.c +++ /dev/null @@ -1,185 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup AirspeedModule Airspeed Module - * @brief Use GPS data to estimate airspeed - * @{ - * - * @file gps_airspeed.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Airspeed module, handles temperature and pressure readings from BMP085 - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * 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 "openpilot.h" -#include "velocitystate.h" -#include "attitudestate.h" -#include "airspeedsensor.h" -#include "airspeedsettings.h" -#include "gps_airspeed.h" -#include "CoordinateConversions.h" -#include - - -// Private constants -#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO -#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO -#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO -#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO -#define COSINE_OF_5_DEG 0.9961947f - -// Private types -struct GPSGlobals { - float RbeCol1_old[3]; - float gpsVelOld_N; - float gpsVelOld_E; - float gpsVelOld_D; - float oldAirspeed; -}; - -// Private variables -static struct GPSGlobals *gps; - -// Private functions -// a simple square inline function based on multiplication faster than powf(x,2.0f) -static inline float Sq(float x) -{ - return x * x; -} - - -/* - * Initialize function loads first data sets, and allocates memory for structure. - */ -void gps_airspeedInitialize() -{ - // This method saves memory in case we don't use the GPS module. - gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); - - // GPS airspeed calculation variables - VelocityStateInitialize(); - VelocityStateData gpsVelData; - VelocityStateGet(&gpsVelData); - - gps->gpsVelOld_N = gpsVelData.North; - gps->gpsVelOld_E = gpsVelData.East; - gps->gpsVelOld_D = gpsVelData.Down; - - gps->oldAirspeed = 0.0f; - - AttitudeStateData attData; - AttitudeStateGet(&attData); - - float Rbe[3][3]; - float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; - - // Calculate rotation matrix - Quaternion2R(q, Rbe); - - gps->RbeCol1_old[0] = Rbe[0][0]; - gps->RbeCol1_old[1] = Rbe[0][1]; - gps->RbeCol1_old[2] = Rbe[0][2]; -} - -/* - * Calculate airspeed as a function of GPS groundspeed and vehicle attitude. - * From "IMU Wind Estimation (Theory)", by William Premerlani. - * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => - * V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1. - * If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1), - * where "f" is the fuselage vector in earth coordinates. - * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. - */ -/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is - * not constant, i.e. when the change in V_gps does not solely come from a reorientation - * this error depends strongly on the time scale considered. Is the time between t1 and t2 too - * small, "spikes" absorving unconsidred acceleration will arise - */ -void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) -{ - float Rbe[3][3]; - - { // Scoping to save memory. We really just need Rbe. - AttitudeStateData attData; - AttitudeStateGet(&attData); - - float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; - - // Calculate rotation matrix - Quaternion2R(q, Rbe); - } - - // Calculate the cos(angle) between the two fuselage basis vectors - float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]); - - // If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue. - if (fabsf(cosDiff) < COSINE_OF_5_DEG) { - VelocityStateData gpsVelData; - VelocityStateGet(&gpsVelData); - - if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); - return; // do not calculate if gps velocity is insufficient... - } - - // Calculate the norm^2 of the difference between the two GPS vectors - float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D); - - // Calculate the norm^2 of the difference between the two fuselage vectors - float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]); - - // Airspeed magnitude is the ratio between the two difference norms - float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2); - if (!IS_REAL(airspeedData->CalibratedAirspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); - } else if (!IS_REAL(airspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); - } else { - // need a low pass filter to filter out spikes in non coordinated maneuvers - airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed; - gps->oldAirspeed = airspeedData->CalibratedAirspeed; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); - } - - // Save old variables for next pass - gps->gpsVelOld_N = gpsVelData.North; - gps->gpsVelOld_E = gpsVelData.East; - gps->gpsVelOld_D = gpsVelData.Down; - - gps->RbeCol1_old[0] = Rbe[0][0]; - gps->RbeCol1_old[1] = Rbe[0][1]; - gps->RbeCol1_old[2] = Rbe[0][2]; - } -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Airspeed/inc/gps_airspeed.h b/flight/modules/Airspeed/inc/gps_airspeed.h deleted file mode 100644 index 4b5f08ad6..000000000 --- a/flight/modules/Airspeed/inc/gps_airspeed.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup AirspeedModule Airspeed Module - * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements - * @{ - * - * @file gps_airspeed.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Airspeed module, reads temperature and pressure from BMP085 - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * 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 GPS_AIRSPEED_H -#define GPS_AIRSPEED_H - -void gps_airspeedInitialize(); -void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); - -#endif // GPS_AIRSPEED_H - -/** - * @} - * @} - */ diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index f61cb63fc..cc9341a2d 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -7,6 +7,12 @@ + + + + + + From 530c0aa967e09c1e330204f620ad55162e5003ec Mon Sep 17 00:00:00 2001 From: Andres <> Date: Sat, 10 May 2014 14:51:22 +0200 Subject: [PATCH 017/718] OP-1317 Added two file (actually renames) --- flight/modules/Airspeed/imu_airspeed.c | 217 +++++++++++++++++++++ flight/modules/Airspeed/inc/imu_airspeed.h | 42 ++++ 2 files changed, 259 insertions(+) create mode 100644 flight/modules/Airspeed/imu_airspeed.c create mode 100644 flight/modules/Airspeed/inc/imu_airspeed.h diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c new file mode 100644 index 000000000..9f887feb6 --- /dev/null +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -0,0 +1,217 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Use attitude and velocity data to estimate airspeed + * @{ + * + * @file imu_airspeed.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief IMU based airspeed calculation + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 "openpilot.h" +#include "velocitystate.h" +#include "attitudestate.h" +#include "airspeedsensor.h" +#include "airspeedsettings.h" +#include "imu_airspeed.h" +#include "CoordinateConversions.h" +#include + + +// Private constants +#define EPS_REORIENTATION 1e-8f +#define EPS_VELOCITY 1.f + +// Private types +// structure with smoothed fuselage orientation, ground speed and their changes in time +struct IMUGlobals { + float xB[3]; + float dxB[3]; + float Vel[3]; + float dVel[3]; +}; + +// Private variables +static struct IMUGlobals *imu; + +// Private functions +// a simple square inline function based on multiplication faster than powf(x,2.0f) +static inline float Sq(float x) +{ + return x * x; +} + +/* + * Initialize function loads first data sets, and allocates memory for structure. + */ +void imu_airspeedInitialize() +{ + // This method saves memory in case we don't use the module. + imu = (struct IMUGlobals *)pvPortMalloc(sizeof(struct IMUGlobals)); + + // airspeed calculation variables + VelocityStateInitialize(); + VelocityStateData velData; + VelocityStateGet(&velData); + + AttitudeStateData attData; + AttitudeStateGet(&attData); + + // for Holt-Winters double exponential smoothing (s smooth variable, b smooth trend) + // s1 = x1 + // b1 = x1 - x0 + // Calculate x of body frame + QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, imu->xB); + + // ground speed + imu->Vel[0] = velData.North; + imu->Vel[1] = velData.East; + imu->Vel[2] = velData.Down; + + // trend assumed to be zero + imu->dxB[0] = imu->dxB[1] = imu->dxB[2] = 0.f; + imu->dVel[0] = imu->dVel[1] = imu->dVel[2] = 0.f; +} + +/* + * Calculate airspeed as a function of groundspeed and vehicle attitude. + * Adapted from "IMU Wind Estimation (Theory)", by William Premerlani. + * The idea is that V_gps=V_air+V_wind. If we assume wind constant, => + * V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1. + * If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1), + * where "f" is the fuselage vector in earth coordinates. + * We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|. + * Adapted to: |V| = (V_gps_2-V_gps_1) dot (f2_-f_1) / |f_2 - f1|^2. + * + * See OP-1317 imu_wind_estimation.pdf for details on the adaptation + * Need a low pass filter to filter out spikes in non coordinated maneuvers + * Note: filtering of xB and gpsV is more effective than of the airspeed itself. Reason: derivative of oscillating part is scaled + * by 1/period, i.e. fast oscillation => small period => large oscillation in derivative => large oscillation in airspeed + * Idea: treat gpsV and xB as noisy time series with trend and + * apply Holt-Winters double exponential smoothing to avoid smoothing out of trend (=derivative) + * s1 = x1 + * b1 = x1 - x0 + * s_{k+1} = alpha*x_{k+1} + (1-alpha)*(s_k + b_k) + * b_{k+1} = beta*(s_{k+1} - s_k) + (1-beta)b_k + */ +void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) +{ + const float alpha = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha; + const float beta = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha; + + // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed + { // Scoping to save memory + float xB[3]; + AttitudeStateData attData; + AttitudeStateGet(&attData); + VelocityStateData velData; + VelocityStateGet(&velData); + + + // Calculate rotation matrix + QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, xB); + + // Holt-Winters double exponential smoothing + // Orientation xB + float sk = imu->xB[0]; + imu->xB[0] = alpha * xB[0] + (1.f - alpha) * (sk + imu->dxB[0]); + imu->dxB[0] = beta * (imu->xB[0] - sk) + (1.f - beta) * imu->dxB[0]; + + sk = imu->xB[1]; + imu->xB[1] = alpha * xB[1] + (1.f - alpha) * (sk + imu->dxB[1]); + imu->dxB[1] = beta * (imu->xB[1] - sk) + (1.f - beta) * imu->dxB[1]; + + sk = imu->xB[2]; + imu->xB[2] = alpha * xB[2] + (1.f - alpha) * (sk + imu->dxB[2]); + imu->dxB[2] = beta * (imu->xB[2] - sk) + (1.f - beta) * imu->dxB[2]; + + // Ground speed Vel + sk = imu->Vel[0]; + imu->Vel[0] = alpha * velData.North + (1.f - alpha) * (sk + imu->dVel[0]); + imu->dVel[0] = beta * (imu->Vel[0] - sk) + (1.f - beta) * imu->dVel[0]; + + sk = imu->Vel[1]; + imu->Vel[1] = alpha * velData.East + (1.f - alpha) * (sk + imu->dVel[1]); + imu->dVel[1] = beta * (imu->Vel[1] - sk) + (1.f - beta) * imu->dVel[1]; + + sk = imu->Vel[2]; + imu->Vel[2] = alpha * velData.Down + (1.f - alpha) * (sk + imu->dVel[2]); + imu->dVel[2] = beta * (imu->Vel[2] - sk) + (1.f - beta) * imu->dVel[2]; + + /////// for debugging purposes only! //////////// + airspeedData->f[0] = imu->xB[0]; + airspeedData->f[1] = imu->xB[1]; + airspeedData->f[2] = imu->xB[2]; + + airspeedData->v[0] = imu->Vel[0]; + airspeedData->v[1] = imu->Vel[1]; + airspeedData->v[2] = imu->Vel[2]; + + airspeedData->df[0] = imu->dxB[0]; + airspeedData->df[1] = imu->dxB[1]; + airspeedData->df[2] = imu->dxB[2]; + + airspeedData->dv[0] = imu->dVel[0]; + airspeedData->dv[1] = imu->dVel[1]; + airspeedData->dv[2] = imu->dVel[2]; + airspeedData->absdf = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]); + airspeedData->dvdotdf = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2]; + ////////////////////////////////////////////////// + } + + // Calculate the norm^2 of the difference between the two fuselage vectors + const float normDiffAttitude2 = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]); + const float normVel2 = Sq(imu->Vel[0]) + Sq(imu->Vel[1]) + Sq(imu->Vel[2]); + + // Some reorientation needed to be able to calculate airspeed and calculate only for sufficient velocity + if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY) { + // Calculate scalar product of difference vectors + const float dvdtDotdfdt = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2]; + + // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2 + // airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt + const float airspeed = dvdtDotdfdt / normDiffAttitude2; + + if (!IS_REAL(airspeedData->CalibratedAirspeed)) { + airspeedData->CalibratedAirspeed = 0; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); + } else { + airspeedData->CalibratedAirspeed = airspeed; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); + } + } else { + airspeedData->CalibratedAirspeed = 0; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); + } +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/Airspeed/inc/imu_airspeed.h b/flight/modules/Airspeed/inc/imu_airspeed.h new file mode 100644 index 000000000..88fe73b79 --- /dev/null +++ b/flight/modules/Airspeed/inc/imu_airspeed.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed as a function of the difference between sequential ground velocity and attitude measurements + * @{ + * + * @file imu_airspeed.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Airspeed module, reads temperature and pressure from BMP085 + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 IMU_AIRSPEED_H +#define IMU_AIRSPEED_H + +void imu_airspeedInitialize(); +void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); + +#endif // IMU_AIRSPEED_H + +/** + * @} + * @} + */ From c5372dd0b4b9cf155f3d56ae60cb880e9c7b75f6 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Tue, 13 May 2014 17:30:50 +0200 Subject: [PATCH 018/718] OP-1317 Added better comments on the function of the partial calculation of the rotation matrix. And added a remark on the debuging variable in the airspeed object definition --- flight/libraries/CoordinateConversions.c | 9 ++++++--- flight/libraries/inc/CoordinateConversions.h | 9 ++++++--- shared/uavobjectdefinition/airspeedsensor.xml | 2 ++ 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/flight/libraries/CoordinateConversions.c b/flight/libraries/CoordinateConversions.c index c3a4f2382..61bf9191b 100644 --- a/flight/libraries/CoordinateConversions.c +++ b/flight/libraries/CoordinateConversions.c @@ -194,7 +194,8 @@ void Quaternion2R(float q[4], float Rbe[3][3]) } -// ** Find x of body frame from quaternion ** +// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the fuselage/roll vector xB ** void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]) { const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; @@ -211,7 +212,8 @@ void Quaternion2xB(const float q[4], float x[3]) } -// ** Find y of body frame from quaternion ** +// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the spanwise/pitch vector yB ** void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]) { const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; @@ -228,7 +230,8 @@ void Quaternion2yB(const float q[4], float y[3]) } -// ** Find z of body frame from quaternion ** +// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the vertical/yaw vector zB ** void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]) { const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3; diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index a2136c73d..d21e29e97 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -50,15 +50,18 @@ void RPY2Quaternion(const float rpy[3], float q[4]); // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** void Quaternion2R(float q[4], float Rbe[3][3]); -// ** Find x of body frame from quaternion ** +// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the fuselage/roll vector xB ** void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]); void Quaternion2xB(const float q[4], float x[3]); -// ** Find y of body frame from quaternion ** +// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the spanwise/pitch vector yB ** void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]); void Quaternion2yB(const float q[4], float y[3]); -// ** Find x of body frame from quaternion ** +// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion ** +// ** This vector corresponds to the vertical/yaw vector zB ** void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]); void Quaternion2zB(const float q[4], float z[3]); diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index cc9341a2d..d0b93a994 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -7,12 +7,14 @@ + + From 41dd85b7414bb84a4120ca7237372d588a5ebf5b Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 21 May 2014 19:25:27 +0200 Subject: [PATCH 019/718] OP-1317 Changed filtering method of IMU airspeed calculation into a Butterworth second order filter --- flight/modules/Airspeed/imu_airspeed.c | 218 +++++++++++++++---------- 1 file changed, 132 insertions(+), 86 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 9f887feb6..230a5fa16 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -47,10 +47,17 @@ // Private types // structure with smoothed fuselage orientation, ground speed and their changes in time struct IMUGlobals { - float xB[3]; - float dxB[3]; - float Vel[3]; - float dVel[3]; + // storage variables for Butterworth filter + float pn1, pn2; + float yn1, yn2; + float v1n1, v1n2; + float v2n1, v2n2; + float v3n1, v3n2; + float Vn1,Vn2; + + // storage variables for derivative calculation + float pOld, yOld; + float v1Old, v2Old, v3Old; }; // Private variables @@ -63,6 +70,63 @@ static inline float Sq(float x) return x * x; } +// ****** find pitch, yaw from quaternion ******** +static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg) +{ + float R13, R11, R12; + const float q0s = q0 * q0; + const float q1s = q1 * q1; + const float q2s = q2 * q2; + const float q3s = q3 * q3; + + R13 = 2.0f * (q1 * q3 - q0 * q2); + R11 = q0s + q1s - q2s - q3s; + R12 = 2.0f * (q1 * q2 + q0 * q3); + + *pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2 + + const float y_=atan2f(R12, R11); + // use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument + // else simply copy atan2 result into result + if(principalArg){ + *yPtr = y_; + }else{ + const int mod=(int)((y_-*yPtr)/(2.0f*M_PI_F*0.9f)); + *yPtr = y_- 2.0f*M_PI_F*mod; + } +} + +static void PY2xB(float p, float y, float x[3]) +{ + const float cosp=cosf(p); + x[0]=cosp*cosf(y); + x[1]=cosp*sinf(y); + x[2]=-sinf(p); +} + + +//second order Butterworth filter with cut-off frequency ratio ff +// filter is writen in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored +// function takes care of updating the values wn1 and wn2 +float FilterButterWorthDF2(const float ff, float xn, float *wn1Ptr, float *wn2Ptr) +{ + // TODO: we need to think about storing the filter instead of calculating it again and again + const float ita =1.0f/ tanf(M_PI_F*ff); + const float q=sqrtf(2.0f); + const float b0 = 1.0f / (1.0f + q*ita + Sq(ita)); + const float b1= 2.0f*b0; + const float b2= b0; + const float a1 = 2.0f * (Sq(ita) - 1.0f) * b0; + const float a2 = -(1.0f - q*ita + Sq(ita)) * b0; + + const float wn=xn + a1*(*wn1Ptr) + a2*(*wn2Ptr); + const float val=b0*wn + b1*(*wn1Ptr) + b2*(*wn2Ptr); + *wn2Ptr=*wn1Ptr; + *wn1Ptr=wn; + return val; +} + + /* * Initialize function loads first data sets, and allocates memory for structure. */ @@ -79,20 +143,18 @@ void imu_airspeedInitialize() AttitudeStateData attData; AttitudeStateGet(&attData); - // for Holt-Winters double exponential smoothing (s smooth variable, b smooth trend) - // s1 = x1 - // b1 = x1 - x0 - // Calculate x of body frame - QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, imu->xB); + // get pitch and yaw from quarternion; principal argument for yaw + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld),&(imu->yOld),true); + + imu->pn1 = imu->pn2 = imu->pOld; + imu->yn1 = imu->yn2 = imu->yOld; + + imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North; + imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East; + imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down; - // ground speed - imu->Vel[0] = velData.North; - imu->Vel[1] = velData.East; - imu->Vel[2] = velData.Down; - - // trend assumed to be zero - imu->dxB[0] = imu->dxB[1] = imu->dxB[2] = 0.f; - imu->dVel[0] = imu->dVel[1] = imu->dVel[2] = 0.f; + // initial guess for airspeed is modulus of groundspeed + imu->Vn1 = imu->Vn2 = sqrt(Sq(velData.North) + Sq(velData.East) + Sq(velData.Down)); } /* @@ -118,91 +180,75 @@ void imu_airspeedInitialize() */ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { - const float alpha = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha; - const float beta = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha; - + //pre-filter frequency rate + //corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec + const float ff=0.04f * 1000.0f/airspeedSettings->SamplePeriod; + // good values for turbulent situation: cut-off 0.01 Hz or a period of 100 sec + const float ffV=0.01f * 1000.0f/airspeedSettings->SamplePeriod; + // good values for steady situation: cut-off 0.05 Hz or a period of 20 sec +// const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod; + + float dxB[3], dVel[3]; + float normVel2; + // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed { // Scoping to save memory - float xB[3]; AttitudeStateData attData; AttitudeStateGet(&attData); VelocityStateData velData; VelocityStateGet(&velData); + float p=imu->pOld, y=imu->yOld; + float xB[3], xBOld[3]; + // get pitch and roll Euler angles from quaternion + // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4,&p,&y,false); - // Calculate rotation matrix - QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, xB); + // filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times + p=FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); + y=FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); + // transform pitch and yaw into fuselage vector xB + PY2xB(imu->pOld,imu->yOld,xBOld); + PY2xB(p,y,xB); + // calculate change in fuselage vector + dxB[0]=xB[0]-xBOld[0]; + dxB[1]=xB[1]-xBOld[1]; + dxB[2]=xB[2]-xBOld[2]; + + // filter ground speed from VelocityState + const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); + const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); + const float fv3n = FilterButterWorthDF2(ff, velData.Down, &(imu->v3n1), &(imu->v3n2)); + // calculate change in ground velocity + dVel[0] = fv1n - imu->v1Old; + dVel[1] = fv2n - imu->v2Old; + dVel[2] = fv3n - imu->v3Old; - // Holt-Winters double exponential smoothing - // Orientation xB - float sk = imu->xB[0]; - imu->xB[0] = alpha * xB[0] + (1.f - alpha) * (sk + imu->dxB[0]); - imu->dxB[0] = beta * (imu->xB[0] - sk) + (1.f - beta) * imu->dxB[0]; - - sk = imu->xB[1]; - imu->xB[1] = alpha * xB[1] + (1.f - alpha) * (sk + imu->dxB[1]); - imu->dxB[1] = beta * (imu->xB[1] - sk) + (1.f - beta) * imu->dxB[1]; - - sk = imu->xB[2]; - imu->xB[2] = alpha * xB[2] + (1.f - alpha) * (sk + imu->dxB[2]); - imu->dxB[2] = beta * (imu->xB[2] - sk) + (1.f - beta) * imu->dxB[2]; - - // Ground speed Vel - sk = imu->Vel[0]; - imu->Vel[0] = alpha * velData.North + (1.f - alpha) * (sk + imu->dVel[0]); - imu->dVel[0] = beta * (imu->Vel[0] - sk) + (1.f - beta) * imu->dVel[0]; - - sk = imu->Vel[1]; - imu->Vel[1] = alpha * velData.East + (1.f - alpha) * (sk + imu->dVel[1]); - imu->dVel[1] = beta * (imu->Vel[1] - sk) + (1.f - beta) * imu->dVel[1]; - - sk = imu->Vel[2]; - imu->Vel[2] = alpha * velData.Down + (1.f - alpha) * (sk + imu->dVel[2]); - imu->dVel[2] = beta * (imu->Vel[2] - sk) + (1.f - beta) * imu->dVel[2]; - - /////// for debugging purposes only! //////////// - airspeedData->f[0] = imu->xB[0]; - airspeedData->f[1] = imu->xB[1]; - airspeedData->f[2] = imu->xB[2]; - - airspeedData->v[0] = imu->Vel[0]; - airspeedData->v[1] = imu->Vel[1]; - airspeedData->v[2] = imu->Vel[2]; - - airspeedData->df[0] = imu->dxB[0]; - airspeedData->df[1] = imu->dxB[1]; - airspeedData->df[2] = imu->dxB[2]; - - airspeedData->dv[0] = imu->dVel[0]; - airspeedData->dv[1] = imu->dVel[1]; - airspeedData->dv[2] = imu->dVel[2]; - airspeedData->absdf = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]); - airspeedData->dvdotdf = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2]; - ////////////////////////////////////////////////// + // calculate norm of ground speed + normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n); + + // actualise old values + imu->pOld = p; imu->yOld = y; + imu->v1Old = fv1n; imu->v2Old = fv2n; imu->v3Old = fv3n; } // Calculate the norm^2 of the difference between the two fuselage vectors - const float normDiffAttitude2 = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]); - const float normVel2 = Sq(imu->Vel[0]) + Sq(imu->Vel[1]) + Sq(imu->Vel[2]); - - // Some reorientation needed to be able to calculate airspeed and calculate only for sufficient velocity - if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY) { - // Calculate scalar product of difference vectors - const float dvdtDotdfdt = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2]; + const float normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]); + // Calculate scalar product of difference vectors + const float dvdtDotdfdt = dVel[0] * dxB[0] + dVel[1] * dxB[1] + dVel[2] * dxB[2]; + // Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity + // a negative scalar product is a clear sign that we are not really able to calculate the airspeed + if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) { // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2 // airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt const float airspeed = dvdtDotdfdt / normDiffAttitude2; - - if (!IS_REAL(airspeedData->CalibratedAirspeed)) { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR); - } else { - airspeedData->CalibratedAirspeed = airspeed; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); - } + // filter raw airspeed + const float fVn=FilterButterWorthDF2(ffV,airspeed,&(imu->Vn1),&(imu->Vn2)); + + airspeedData->CalibratedAirspeed = fVn; + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); } else { airspeedData->CalibratedAirspeed = 0; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; From 423645392b01bc39c8dce597285cb14695c8572a Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 21 May 2014 19:32:02 +0200 Subject: [PATCH 020/718] OP-1317 Changed filtering method of IMU airspeed calculation into a Butterworth second order filter --- flight/modules/Airspeed/imu_airspeed.c | 8 -------- 1 file changed, 8 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 230a5fa16..c18f16a12 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -169,14 +169,6 @@ void imu_airspeedInitialize() * * See OP-1317 imu_wind_estimation.pdf for details on the adaptation * Need a low pass filter to filter out spikes in non coordinated maneuvers - * Note: filtering of xB and gpsV is more effective than of the airspeed itself. Reason: derivative of oscillating part is scaled - * by 1/period, i.e. fast oscillation => small period => large oscillation in derivative => large oscillation in airspeed - * Idea: treat gpsV and xB as noisy time series with trend and - * apply Holt-Winters double exponential smoothing to avoid smoothing out of trend (=derivative) - * s1 = x1 - * b1 = x1 - x0 - * s_{k+1} = alpha*x_{k+1} + (1-alpha)*(s_k + b_k) - * b_{k+1} = beta*(s_{k+1} - s_k) + (1-beta)b_k */ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { From a83379425c7952c49d5ac18030c7b8aefef4c8dc Mon Sep 17 00:00:00 2001 From: Andres <> Date: Thu, 22 May 2014 10:00:50 +0200 Subject: [PATCH 021/718] OP-1317 Small optimizations of code, delete of debug variables in airspeed object and uncrustify --- flight/modules/Airspeed/imu_airspeed.c | 156 +++++++++--------- shared/uavobjectdefinition/airspeedsensor.xml | 8 - 2 files changed, 82 insertions(+), 82 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index c18f16a12..703acf0d1 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -41,6 +41,8 @@ // Private constants +#define TWO_PI 6.283185308f +#define SQRT2 1.414213562f #define EPS_REORIENTATION 1e-8f #define EPS_VELOCITY 1.f @@ -53,13 +55,15 @@ struct IMUGlobals { float v1n1, v1n2; float v2n1, v2n2; float v3n1, v3n2; - float Vn1,Vn2; - - // storage variables for derivative calculation + float Vn1, Vn2; + + + // storage variables for derivative calculation float pOld, yOld; float v1Old, v2Old, v3Old; }; + // Private variables static struct IMUGlobals *imu; @@ -79,50 +83,50 @@ static void Quaternion2PY(const float q0, const float q1, const float q2, const const float q2s = q2 * q2; const float q3s = q3 * q3; - R13 = 2.0f * (q1 * q3 - q0 * q2); - R11 = q0s + q1s - q2s - q3s; - R12 = 2.0f * (q1 * q2 + q0 * q3); + R13 = 2.0f * (q1 * q3 - q0 * q2); + R11 = q0s + q1s - q2s - q3s; + R12 = 2.0f * (q1 * q2 + q0 * q3); *pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2 - const float y_=atan2f(R12, R11); + const float y_ = atan2f(R12, R11); // use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument // else simply copy atan2 result into result - if(principalArg){ + if (principalArg) { *yPtr = y_; - }else{ - const int mod=(int)((y_-*yPtr)/(2.0f*M_PI_F*0.9f)); - *yPtr = y_- 2.0f*M_PI_F*mod; + } else { + // calculate needed mutliples of 2pi to avoid jumps + // take slightly less than 2pi, because the jump will never be exactly 2pi + const int mod = (int)((y_ - *yPtr) / (TWO_PI * 0.9f)); + *yPtr = y_ - TWO_PI * mod; } } static void PY2xB(float p, float y, float x[3]) { - const float cosp=cosf(p); - x[0]=cosp*cosf(y); - x[1]=cosp*sinf(y); - x[2]=-sinf(p); + const float cosp = cosf(p); + + x[0] = cosp * cosf(y); + x[1] = cosp * sinf(y); + x[2] = -sinf(p); } -//second order Butterworth filter with cut-off frequency ratio ff -// filter is writen in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored +// second order Butterworth filter with cut-off frequency ratio ff +// Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored // function takes care of updating the values wn1 and wn2 -float FilterButterWorthDF2(const float ff, float xn, float *wn1Ptr, float *wn2Ptr) +float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float *wn2Ptr) { - // TODO: we need to think about storing the filter instead of calculating it again and again - const float ita =1.0f/ tanf(M_PI_F*ff); - const float q=sqrtf(2.0f); - const float b0 = 1.0f / (1.0f + q*ita + Sq(ita)); - const float b1= 2.0f*b0; - const float b2= b0; - const float a1 = 2.0f * (Sq(ita) - 1.0f) * b0; - const float a2 = -(1.0f - q*ita + Sq(ita)) * b0; - - const float wn=xn + a1*(*wn1Ptr) + a2*(*wn2Ptr); - const float val=b0*wn + b1*(*wn1Ptr) + b2*(*wn2Ptr); - *wn2Ptr=*wn1Ptr; - *wn1Ptr=wn; + const float ita = 1.0f / tanf(M_PI_F * ff); + const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); + const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); + const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); + + const float wn = xn + a1 * (*wn1Ptr) + a2 * (*wn2Ptr); + const float val = b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); + + *wn2Ptr = *wn1Ptr; + *wn1Ptr = wn; return val; } @@ -144,17 +148,17 @@ void imu_airspeedInitialize() AttitudeStateGet(&attData); // get pitch and yaw from quarternion; principal argument for yaw - Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld),&(imu->yOld),true); - - imu->pn1 = imu->pn2 = imu->pOld; - imu->yn1 = imu->yn2 = imu->yOld; - + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true); + + imu->pn1 = imu->pn2 = imu->pOld; + imu->yn1 = imu->yn2 = imu->yOld; + imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North; imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East; imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down; // initial guess for airspeed is modulus of groundspeed - imu->Vn1 = imu->Vn2 = sqrt(Sq(velData.North) + Sq(velData.East) + Sq(velData.Down)); + imu->Vn1 = imu->Vn2 = sqrt(Sq(velData.North) + Sq(velData.East) + Sq(velData.Down)); } /* @@ -169,75 +173,79 @@ void imu_airspeedInitialize() * * See OP-1317 imu_wind_estimation.pdf for details on the adaptation * Need a low pass filter to filter out spikes in non coordinated maneuvers + * A two step Butterworth second order filter is used. In the first step fuselage vector xB + * and ground speed vector Vel are filtered. The fuselage vector is filtered through its pitch + * and yaw to keep a unit length. After building the differenced dxB and dVel are produced and + * the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter */ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { - //pre-filter frequency rate - //corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec - const float ff=0.04f * 1000.0f/airspeedSettings->SamplePeriod; + // pre-filter frequency rate + // corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec + const float ff = 0.04f * 1000.0f / airspeedSettings->SamplePeriod; // good values for turbulent situation: cut-off 0.01 Hz or a period of 100 sec - const float ffV=0.01f * 1000.0f/airspeedSettings->SamplePeriod; + const float ffV = 0.01f * 1000.0f / airspeedSettings->SamplePeriod; // good values for steady situation: cut-off 0.05 Hz or a period of 20 sec -// const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod; - - float dxB[3], dVel[3]; +// const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod; + float normVel2; - + float normDiffAttitude2; + float dvdtDotdfdt; + // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed { // Scoping to save memory AttitudeStateData attData; AttitudeStateGet(&attData); VelocityStateData velData; VelocityStateGet(&velData); - float p=imu->pOld, y=imu->yOld; - float xB[3], xBOld[3]; + float p = imu->pOld, y = imu->yOld; + float dxB[3], xBOld[3]; // get pitch and roll Euler angles from quaternion // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw - Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4,&p,&y,false); + Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false); // filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times - p=FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); - y=FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); - // transform pitch and yaw into fuselage vector xB - PY2xB(imu->pOld,imu->yOld,xBOld); - PY2xB(p,y,xB); + p = FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); + y = FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); + // transform pitch and yaw into fuselage vector xB and xBold + PY2xB(imu->pOld, imu->yOld, xBOld); + PY2xB(p, y, dxB); // calculate change in fuselage vector - dxB[0]=xB[0]-xBOld[0]; - dxB[1]=xB[1]-xBOld[1]; - dxB[2]=xB[2]-xBOld[2]; - + dxB[0] -= xBOld[0]; + dxB[1] -= xBOld[1]; + dxB[2] -= xBOld[2]; + // filter ground speed from VelocityState const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); - const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); - const float fv3n = FilterButterWorthDF2(ff, velData.Down, &(imu->v3n1), &(imu->v3n2)); - // calculate change in ground velocity - dVel[0] = fv1n - imu->v1Old; - dVel[1] = fv2n - imu->v2Old; - dVel[2] = fv3n - imu->v3Old; + const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); + const float fv3n = FilterButterWorthDF2(ff, velData.Down, &(imu->v3n1), &(imu->v3n2)); // calculate norm of ground speed normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n); - - // actualise old values - imu->pOld = p; imu->yOld = y; - imu->v1Old = fv1n; imu->v2Old = fv2n; imu->v3Old = fv3n; - } + // calculate norm of orientation change + normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]); + // cauclate scalar product between groundspeed change and orientation change + dvdtDotdfdt = (fv1n - imu->v1Old) * dxB[0] + (fv2n - imu->v2Old) * dxB[1] + (fv3n - imu->v3Old) * dxB[2]; - // Calculate the norm^2 of the difference between the two fuselage vectors - const float normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]); - // Calculate scalar product of difference vectors - const float dvdtDotdfdt = dVel[0] * dxB[0] + dVel[1] * dxB[1] + dVel[2] * dxB[2]; + // actualise old values + imu->pOld = p; + imu->yOld = y; + imu->v1Old = fv1n; + imu->v2Old = fv2n; + imu->v3Old = fv3n; + } // Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity // a negative scalar product is a clear sign that we are not really able to calculate the airspeed + // NOTE: normVel2 check against EPS_VELOCITY might make problems during hovering maneuvers in fixed wings if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) { // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2 // airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt const float airspeed = dvdtDotdfdt / normDiffAttitude2; // filter raw airspeed - const float fVn=FilterButterWorthDF2(ffV,airspeed,&(imu->Vn1),&(imu->Vn2)); - + const float fVn = FilterButterWorthDF2(ffV, airspeed, &(imu->Vn1), &(imu->Vn2)); + airspeedData->CalibratedAirspeed = fVn; airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); diff --git a/shared/uavobjectdefinition/airspeedsensor.xml b/shared/uavobjectdefinition/airspeedsensor.xml index d0b93a994..f61cb63fc 100644 --- a/shared/uavobjectdefinition/airspeedsensor.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -7,14 +7,6 @@ - - - - - - - - From 822b067d98e53251aab349f17e3217bbdcbb4417 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Fri, 23 May 2014 07:37:57 +0200 Subject: [PATCH 022/718] OP-1317 Corrected error in calculation of low pass filter cut-off frequencies --- flight/modules/Airspeed/imu_airspeed.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 703acf0d1..baf2c6a52 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -182,9 +182,9 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air { // pre-filter frequency rate // corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec - const float ff = 0.04f * 1000.0f / airspeedSettings->SamplePeriod; + const float ff = 0.04f * airspeedSettings->SamplePeriod / 1000.0f; // good values for turbulent situation: cut-off 0.01 Hz or a period of 100 sec - const float ffV = 0.01f * 1000.0f / airspeedSettings->SamplePeriod; + const float ffV = 0.01f * airspeedSettings->SamplePeriod / 1000.0f; // good values for steady situation: cut-off 0.05 Hz or a period of 20 sec // const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod; From 9e80c4da9c2f895f0de74490639b86b2ed3894b4 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Fri, 23 May 2014 11:55:06 +0200 Subject: [PATCH 023/718] OP-1317 Low-pass filter parameters added to airpeedSettings: IMUBasedEstimationLowPassPeriod1 pre-filter period in seconds, IMUBasedEstimationLowPassPeriod2 filter period in seconds. Oscillations with shorter periods than these, are filtered. If airspeed has too much noise increase IMUBasedEstimationLowPassPeriod2, else if it is too smooth descrease it. --- flight/modules/Airspeed/imu_airspeed.c | 9 +++------ shared/uavobjectdefinition/airspeedsettings.xml | 3 ++- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index baf2c6a52..65158fbb2 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -181,12 +181,9 @@ void imu_airspeedInitialize() void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) { // pre-filter frequency rate - // corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec - const float ff = 0.04f * airspeedSettings->SamplePeriod / 1000.0f; - // good values for turbulent situation: cut-off 0.01 Hz or a period of 100 sec - const float ffV = 0.01f * airspeedSettings->SamplePeriod / 1000.0f; - // good values for steady situation: cut-off 0.05 Hz or a period of 20 sec -// const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod; + const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1; + // filter frequency rate + const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2; float normVel2; float normDiffAttitude2; diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 0c438afa5..00cd56241 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -5,7 +5,8 @@ - + + From 642f740be315a919e657128f2d8ed1433e85ba9d Mon Sep 17 00:00:00 2001 From: Andres <> Date: Sat, 24 May 2014 10:04:39 +0200 Subject: [PATCH 024/718] OP-1317 Bettered jump treatment in yaw calculation and some cosmetics --- flight/modules/Airspeed/imu_airspeed.c | 34 +++++++++++++++++--------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 65158fbb2..9c5ffd789 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -43,7 +43,7 @@ // Private constants #define TWO_PI 6.283185308f #define SQRT2 1.414213562f -#define EPS_REORIENTATION 1e-8f +#define EPS_REORIENTATION 1e-10f #define EPS_VELOCITY 1.f // Private types @@ -96,13 +96,16 @@ static void Quaternion2PY(const float q0, const float q1, const float q2, const *yPtr = y_; } else { // calculate needed mutliples of 2pi to avoid jumps - // take slightly less than 2pi, because the jump will never be exactly 2pi - const int mod = (int)((y_ - *yPtr) / (TWO_PI * 0.9f)); - *yPtr = y_ - TWO_PI * mod; + // number of cycles accumulated in old yaw + const int cycles = (int)(*yPtr / TWO_PI); + // look for a jump by substracting the modulus, i.e. there is maximally one jump. + // take slightly less than 2pi, because the jump will always be lower than 2pi + const int mod = (int)((y_ - (*yPtr - cycles * TWO_PI)) / (TWO_PI * 0.9f)); + *yPtr = y_ + TWO_PI * (cycles - mod); } } -static void PY2xB(float p, float y, float x[3]) +static void PY2xB(const float p, const float y, float x[3]) { const float cosp = cosf(p); @@ -112,6 +115,16 @@ static void PY2xB(float p, float y, float x[3]) } +static void PY2DeltaxB(const float p, const float y, float x[3]) +{ + const float cosp = cosf(p); + + x[0] -= cosp * cosf(y); + x[1] -= cosp * sinf(y); + x[2] -= -sinf(p); +} + + // second order Butterworth filter with cut-off frequency ratio ff // Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored // function takes care of updating the values wn1 and wn2 @@ -196,7 +209,7 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air VelocityStateData velData; VelocityStateGet(&velData); float p = imu->pOld, y = imu->yOld; - float dxB[3], xBOld[3]; + float dxB[3]; // get pitch and roll Euler angles from quaternion // do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw @@ -206,13 +219,10 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air p = FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); y = FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); // transform pitch and yaw into fuselage vector xB and xBold - PY2xB(imu->pOld, imu->yOld, xBOld); PY2xB(p, y, dxB); - // calculate change in fuselage vector - dxB[0] -= xBOld[0]; - dxB[1] -= xBOld[1]; - dxB[2] -= xBOld[2]; - + // calculate change in fuselage vector by substraction of old value + PY2DeltaxB(imu->pOld, imu->yOld, dxB); + // filter ground speed from VelocityState const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); From 4d250ff8566380330f0f5ed79ce0ddcd65d90314 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 25 May 2014 15:20:04 -0400 Subject: [PATCH 025/718] Fix channel indexes to use servoCenterSlider Set channelNeutral = 1500 where appropriate for each model type. Add the missing actuator save for fixed wing! actSettings->setData(data); --- .../pages/outputcalibrationpage.cpp | 37 +++++++++---------- .../vehicleconfigurationhelper.cpp | 10 +++-- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 9bfdeba9c..dba143375 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -139,47 +139,46 @@ void OutputCalibrationPage::setupVehicle() break; case SetupWizard::FIXED_WING_AILERON: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 3 << 4 << 2 << 3 << 4 << 2 << 3 << 4 << 2 << 3 << 4; + m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; //2 for servoCenterSlider! m_vehicleElementIds << "aileron" << "aileron-frame" << "aileron-motor" << "aileron-ail-left" << "aileron-ail-right" << "aileron-rudder" << "aileron-elevator"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 2 << 2 << 3 << 3 << 3 << 4 << 4 << 4 << 5 << 5 << 5; - m_channelIndex << 0 << 0 << 1 << 1 << 1 << 2 << 2 << 2 << 3 << 3 << 3 << 4 << 4 << 4; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; // see Servo city for an example. 1500 usec is center on MS85mg for example. // - http://www.servocity.com/html/hs-85mg__mighty_micro.html // make sure Aileron servo one does not go to an extreme value - m_actuatorSettings[1].channelMin = 1500; + // would also be nice to make these all default to 50hz so we don't shred servos. + m_actuatorSettings[1].channelNeutral = 1500; - m_actuatorSettings[1].channelMax = 1500; // make sure Aileron servo two does not go to an extreme value - m_actuatorSettings[2].channelMin = 1500; m_actuatorSettings[2].channelNeutral = 1500; - m_actuatorSettings[2].channelMax = 1500; // make sure Elevator servo one does not go to an extreme value - m_actuatorSettings[3].channelMin = 1500; - m_actuatorSettings[4].channelNeutral = 1500; - m_actuatorSettings[3].channelMax = 1500; + m_actuatorSettings[3].channelNeutral = 1500; // make sure Rudder servo one does not go to an extreme value - m_actuatorSettings[4].channelMin = 1500; m_actuatorSettings[4].channelNeutral = 1500; - m_actuatorSettings[4].channelMax = 1500; + m_actuatorSettings[4].channelNeutral = 1500; + m_actuatorSettings[5].channelNeutral = 1500; + m_actuatorSettings[6].channelNeutral = 1500; + m_actuatorSettings[7].channelNeutral = 1500; getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::FIXED_WING_VTAIL: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 3 << 4 << 2 << 3 << 4; + m_wizardIndexes << 0 << 1 << 2 << 2; //2 for servoCenterSlider! m_vehicleElementIds << "v-tail" << "v-tail-frame" << "v-tail-motor" << "v-tail-elevon-left" << "v-tail-elevon-right"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 2 << 2 << 3 << 3 << 3; - m_channelIndex << 0 << 0 << 1 << 1 << 1 << 2 << 2 << 2; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3; + m_channelIndex << 0 << 0 << 1 << 2; // make sure elevon servo one does not go to an extreme value - m_actuatorSettings[1].channelMin = 1500; m_actuatorSettings[1].channelNeutral = 1500; - m_actuatorSettings[1].channelMax = 1500; // make sure elevon servo two does not go to an extreme value - m_actuatorSettings[2].channelMin = 1500; m_actuatorSettings[2].channelNeutral = 1500; - m_actuatorSettings[2].channelMax = 1500; + m_actuatorSettings[3].channelNeutral = 1500; + m_actuatorSettings[4].channelNeutral = 1500; + m_actuatorSettings[5].channelNeutral = 1500; + m_actuatorSettings[6].channelNeutral = 1500; + m_actuatorSettings[7].channelNeutral = 1500; getWizard()->setActuatorSettings(m_actuatorSettings); break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 5de6ae900..ed8969589 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -306,7 +306,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() addModifiedObject(actSettings, tr("Writing actuator settings")); break; } -/* + case VehicleConfigurationSource::VEHICLE_FIXEDWING: { ActuatorSettings::DataFields data = actSettings->getData(); @@ -317,12 +317,14 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() for (quint16 i = 1; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelAddr[i] = i; - data.ChannelMin[i] = 1500; data.ChannelNeutral[i] = 1500; - data.ChannelMax[i] = 1500; } + qDebug() << "Save Fixed Wing Actuator Data\n"; + actSettings->setData(data); + addModifiedObject(actSettings, tr("Writing actuator settings")); + break; } -*/ + case VehicleConfigurationSource::VEHICLE_HELI: case VehicleConfigurationSource::VEHICLE_SURFACE: // TODO: Implement settings for other vehicle types? From 4ebaa6f20536a694a13b25d61d6b471cc8fd4072 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 25 May 2014 16:05:13 -0400 Subject: [PATCH 026/718] set all the min and max values to 554 and 2400 per the Arduino servo library fix some cosmetic errors on the debug output fix the Vtail mixer setting that caused channel 4 to go crazy due to being undefined. Servo would sit and shake for no reason. --- .../setupwizard/outputcalibrationutil.cpp | 10 ++--- .../pages/outputcalibrationpage.cpp | 38 ++++++++++++++++++- .../vehicleconfigurationhelper.cpp | 19 +++++++++- 3 files changed, 60 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp index e75b95b2b..9cbf911fb 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp @@ -52,7 +52,7 @@ void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValu m_outputChannel = channel; m_safeValue = safeValue; - qDebug() << "Starting output for channel " << m_outputChannel << "..."; + qDebug() << "Starting output for channel " << m_outputChannel+1 << "..."; ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); Q_ASSERT(actuatorCommand); @@ -74,17 +74,17 @@ void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValu actuatorCommand->setMetadata(metaData); actuatorCommand->updated(); - qDebug() << "Output for channel " << m_outputChannel << " started."; + qDebug() << "Output for channel " << m_outputChannel+1 << " started."; } } void OutputCalibrationUtil::stopChannelOutput() { if (m_outputChannel >= 0) { - qDebug() << "Stopping output for channel " << m_outputChannel << "..."; + qDebug() << "Stopping output for channel " << m_outputChannel+1 << "..."; // Stop output... setChannelOutputValue(m_safeValue); - qDebug() << "Settings output for channel " << m_outputChannel << " to " << m_safeValue; + qDebug() << "Settings output for channel " << m_outputChannel+1 << " to " << m_safeValue; // Restore metadata to what it was before ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager); @@ -93,7 +93,7 @@ void OutputCalibrationUtil::stopChannelOutput() actuatorCommand->setMetadata(m_savedActuatorCommandMetadata); actuatorCommand->updated(); - qDebug() << "Output for channel " << m_outputChannel << " stopped."; + qDebug() << "Output for channel " << m_outputChannel+1 << " stopped."; m_outputChannel = -1; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index dba143375..5f5c9dd9c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -161,6 +161,24 @@ void OutputCalibrationPage::setupVehicle() m_actuatorSettings[6].channelNeutral = 1500; m_actuatorSettings[7].channelNeutral = 1500; + // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, + // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU + // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg + m_actuatorSettings[1].channelMin = 554; + m_actuatorSettings[1].channelMax = 2400; + m_actuatorSettings[2].channelMin = 554; + m_actuatorSettings[2].channelMax = 2400; + m_actuatorSettings[3].channelMin = 554; + m_actuatorSettings[3].channelMax = 2400; + m_actuatorSettings[4].channelMin = 554; + m_actuatorSettings[4].channelMax = 2400; + m_actuatorSettings[5].channelMin = 554; + m_actuatorSettings[5].channelMax = 2400; + m_actuatorSettings[6].channelMin = 554; + m_actuatorSettings[6].channelMax = 2400; + m_actuatorSettings[7].channelMin = 554; + m_actuatorSettings[7].channelMax = 2400; + getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::FIXED_WING_VTAIL: @@ -180,6 +198,24 @@ void OutputCalibrationPage::setupVehicle() m_actuatorSettings[6].channelNeutral = 1500; m_actuatorSettings[7].channelNeutral = 1500; + // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, + // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU + // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg + m_actuatorSettings[1].channelMin = 554; + m_actuatorSettings[1].channelMax = 2400; + m_actuatorSettings[2].channelMin = 554; + m_actuatorSettings[2].channelMax = 2400; + m_actuatorSettings[3].channelMin = 554; + m_actuatorSettings[3].channelMax = 2400; + m_actuatorSettings[4].channelMin = 554; + m_actuatorSettings[4].channelMax = 2400; + m_actuatorSettings[5].channelMin = 554; + m_actuatorSettings[5].channelMax = 2400; + m_actuatorSettings[6].channelMin = 554; + m_actuatorSettings[6].channelMax = 2400; + m_actuatorSettings[7].channelMin = 554; + m_actuatorSettings[7].channelMax = 2400; + getWizard()->setActuatorSettings(m_actuatorSettings); break; default: @@ -255,7 +291,7 @@ void OutputCalibrationPage::setWizardPage() ui->calibrationStack->setCurrentIndex(currentPageIndex); int currentChannel = getCurrentChannel(); - qDebug() << "Current channel: " << currentChannel; + qDebug() << "Current channel: " << currentChannel+1; if (currentChannel >= 0) { if (currentPageIndex == 1) { ui->motorNeutralSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index ed8969589..07341fb94 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -311,13 +311,16 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() { ActuatorSettings::DataFields data = actSettings->getData(); - qDebug() << "Override center pulse for fixed wing servos\n"; + qDebug() << "Override center, min and max pulses for fixed wing servos\n"; // move all but first chan to 1500 center pluse QList actuatorSettings = m_configSource->getActuatorSettings(); for (quint16 i = 1; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelAddr[i] = i; + data.ChannelMin[i] = 554; // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, + // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU data.ChannelNeutral[i] = 1500; + data.ChannelMax[i] = 2400; // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg } qDebug() << "Save Fixed Wing Actuator Data\n"; actSettings->setData(data); @@ -1346,6 +1349,20 @@ void VehicleConfigurationHelper::setupVtail() channels[2].pitch = -50; channels[2].yaw = 0; + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 0; + + channels[3].type = MIXER_TYPE_SERVO; + channels[3].throttle1 = 0; + channels[3].throttle2 = 0; + channels[3].roll = 0; + channels[3].pitch = 0; + channels[3].yaw = 0; + guiSettings.fixedwing.FixedWingThrottle = 1; guiSettings.fixedwing.FixedWingRoll1 = 2; guiSettings.fixedwing.FixedWingRoll2 = 3; From 11fa414e1a9efd6c32f52d43ca04bd5fef5e5048 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Thu, 29 May 2014 11:51:20 -0400 Subject: [PATCH 027/718] Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties. Likewise adjust fixedwingpage.cpp to be more consistant with the inkscape label. ie 'vtail' vs. 'v-tail' --- .../src/plugins/config/images/fixedwing-shapes.svg | 12 ++++++------ .../src/plugins/setupwizard/pages/fixedwingpage.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index 26045315f..dc20b635d 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -19,7 +19,7 @@ viewBox="0 0 792 2016" enable-background="new 0 0 792 1008" xml:space="preserve" - inkscape:version="0.48.4 r9939" + inkscape:version="0.48.2 r9819" sodipodi:docname="fixedwing-shapes.svg">image/svg+xml Date: Sat, 7 Jun 2014 22:31:42 -0400 Subject: [PATCH 028/718] Try to fix my Vtail->Elevon naming issue. Attempt to fix Elevon mixer setting defaults --- .../configfixedwingwidget.cpp | 72 +++++++++---------- .../config/images/fixedwing-shapes.svg | 12 ++-- .../setupwizard/pages/fixedwingpage.cpp | 6 +- .../pages/outputcalibrationpage.cpp | 4 +- .../resources/fixedwing-shapes.svg | 40 +++++------ 5 files changed, 63 insertions(+), 71 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 07ba0a4ba..16eb7fa90 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -88,7 +88,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : populateChannelComboBoxes(); QStringList fixedWingTypes; - fixedWingTypes << "Elevator aileron rudder" << "Vtail"; + fixedWingTypes << "Elevator aileron rudder" << "Elevon"; m_aircraft->fixedWingType->addItems(fixedWingTypes); // Set default model to "Elevator aileron rudder" @@ -138,16 +138,16 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); - } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { - plane->setElementId("vtail"); - setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Vtail")); + } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { + plane->setElementId("elevon"); + setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); m_aircraft->fwRudder1ChannelBox->setEnabled(false); m_aircraft->fwRudder2ChannelBox->setEnabled(false); - m_aircraft->fwElevator1Label->setText("Vtail 1"); + m_aircraft->fwElevator1Label->setText("Elevon 1"); m_aircraft->fwElevator1ChannelBox->setEnabled(true); - m_aircraft->fwElevator2Label->setText("Vtail 2"); + m_aircraft->fwElevator2Label->setText("Elevon 2"); m_aircraft->fwElevator2ChannelBox->setEnabled(true); m_aircraft->fwAileron1Label->setText("Aileron 1"); @@ -182,7 +182,7 @@ void ConfigFixedWingWidget::setupEnabledControls(QString frameType) } } - if (frameType == "Vtail" || frameType == "vtail") { + if (frameType == "Elevon" || frameType == "elevon") { enableComboBoxes(this, CHANNELBOXNAME, 3, true); } else if (frameType == "aileron" || frameType == "Elevator aileron rudder") { enableComboBoxes(this, CHANNELBOXNAME, 4, true); @@ -253,8 +253,8 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) setComboCurrentIndex(m_aircraft->fwRudder1ChannelBox, fixed.FixedWingYaw1); setComboCurrentIndex(m_aircraft->fwRudder2ChannelBox, fixed.FixedWingYaw2); - if (frameType == "FixedWingVtail") { - // If the airframe is vtail, restore the slider setting + if (frameType == "FixedWingElevon") { + // If the airframe is elevon, restore the slider setting // Find the channel number for Elevon1 (FixedWingRoll1) int channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; if (channel > -1) { @@ -297,9 +297,9 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() m_aircraft->fwStatusLabel->setText(tr("Configuration OK")); } - else if (m_aircraft->fixedWingType->currentText() == "vtail") { - airframeType = "FixedWingVtail"; - setupFrameVtail(airframeType); + else if (m_aircraft->fixedWingType->currentText() == "elevon") { + airframeType = "FixedWingElevon"; + setupFrameElevon(airframeType); motor_servo_List << "FixedWingThrottle" << "FixedWingRoll1" << "FixedWingRoll2"; setupMotors(motor_servo_List); @@ -307,7 +307,7 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() GUIConfigDataUnion config = getConfigData(); setConfigData(config); - // Vtail Layout: + // Elevon Layout: // pitch roll yaw double mixerMatrix[8][3] = { { 0, 0, 0 }, @@ -451,9 +451,9 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) } /** - Setup VTail + Setup Elevon */ -bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) +bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) { // Check coherence: // Show any config errors in GUI @@ -465,7 +465,7 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) resetActuators(&config); config.fixedwing.FixedWingPitch1 = m_aircraft->fwElevator1ChannelBox->currentIndex(); - config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); + config.fixedwing.FixedWingPitch2 = m_aircraft->fwElevator2ChannelBox->currentIndex(); config.fixedwing.FixedWingRoll1 = m_aircraft->fwAileron1ChannelBox->currentIndex(); config.fixedwing.FixedWingRoll2 = m_aircraft->fwAileron2ChannelBox->currentIndex(); config.fixedwing.FixedWingThrottle = m_aircraft->fwEngineChannelBox->currentIndex(); @@ -483,6 +483,8 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) // 1. Assign the servo/motor/none for each channel + double value; + // motor int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); @@ -501,36 +503,27 @@ bool ConfigFixedWingWidget::setupFrameVtail(QString airframeType) channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, 127); - - channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -127); - } - - // vtail - channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; - if (channel > -1) { - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - double value = (double)(m_aircraft->elevonSlider2->value() * 1.27); + value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, value); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value); - channel = m_aircraft->fwElevator2ChannelBox->currentIndex() - 1; + + channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); value = (double)(m_aircraft->elevonSlider2->value() * 1.27); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -value); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -value); } m_aircraft->fwStatusLabel->setText("Mixer generated"); return true; + } /** - This function sets up the vtail fixed wing mixer values. + This function sets up the elevon fixed wing mixer values. */ bool ConfigFixedWingWidget::setupFixedWingMixer(double mixerFactors[8][3]) { @@ -603,7 +596,7 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - } else if (airframeType == "FixedWingVtail") { + } else if (airframeType == "FixedWingElevon") { if (m_aircraft->fwEngineChannelBox->currentText() == "None") { m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; @@ -611,21 +604,20 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { - m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + if (m_aircraft->fwAileron1ChannelBox->currentText() == "None") { + m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - if (m_aircraft->fwElevator2ChannelBox->currentText() == "None") { - m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + if (m_aircraft->fwAileron2ChannelBox->currentText() == "None") { + m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } - if (error) { m_aircraft->fwStatusLabel->setText(QString("ERROR: Assign all necessary channels")); } diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index dc20b635d..e518dd1f3 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -1100,14 +1100,14 @@ id="path8516_1_" inkscape:connector-curvature="0" style="fill:none;stroke:#010101;stroke-width:0.69520003;stroke-linecap:round;stroke-linejoin:round" /> \ No newline at end of file + style="fill:none;stroke:#010101;stroke-width:0.69520003;stroke-linecap:round;stroke-linejoin:round" /> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp index 9dd9b9638..f2d8f27ab 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -85,7 +85,7 @@ void FixedWingPage::setupFixedWingTypesCombo() ui->typeCombo->addItem(tr("Aileron, Elevator, Rudder"), SetupWizard::FIXED_WING_AILERON); m_descriptions << tr("This setup currently expects a traditional 4 channel setup including two ailerons (not connected by Y adapter), an elevator and a rudder. "); - ui->typeCombo->addItem(tr("V-Tail"), SetupWizard::FIXED_WING_VTAIL); + ui->typeCombo->addItem(tr("Elevon"), SetupWizard::FIXED_WING_ELEVON); m_descriptions << tr("This setup currently expects a flying-wing setup, an elevon plus rudder setup is not yet supported. Setup should include only two elevons, and should explicitly not include a rudder."); } @@ -105,8 +105,8 @@ void FixedWingPage::updateImageAndDescription() case SetupWizard::FIXED_WING_AILERON: elementId = "aileron"; break; - case SetupWizard::FIXED_WING_VTAIL: - elementId = "vtail"; + case SetupWizard::FIXED_WING_ELEVON: + elementId = "elevon"; break; default: elementId = ""; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index 5f5c9dd9c..8021bfcdc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -181,10 +181,10 @@ void OutputCalibrationPage::setupVehicle() getWizard()->setActuatorSettings(m_actuatorSettings); break; - case SetupWizard::FIXED_WING_VTAIL: + case SetupWizard::FIXED_WING_ELEVON: loadSVGFile(FIXEDWING_SVG_FILE); m_wizardIndexes << 0 << 1 << 2 << 2; //2 for servoCenterSlider! - m_vehicleElementIds << "v-tail" << "v-tail-frame" << "v-tail-motor" << "v-tail-elevon-left" << "v-tail-elevon-right"; + m_vehicleElementIds << "elevon" << "elevon-frame" << "elevon-motor" << "elevon-left" << "elevon-right"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3; m_channelIndex << 0 << 0 << 1 << 2; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg index b0f5b5aec..0ed305c34 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/setupwizard/resources/fixedwing-shapes.svg @@ -1828,24 +1828,24 @@ id="tspan9117" sodipodi:role="line">4 \ No newline at end of file + id="path3961-7-5" /> From 0fa69ba7892a0d4b335aa00ee638f4d00b19a8cf Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sat, 7 Jun 2014 22:52:28 -0400 Subject: [PATCH 029/718] add proper header definition for Elevon setup --- .../src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 56a386daf..cd8fd4611 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -66,7 +66,7 @@ private: virtual void resetActuators(GUIConfigDataUnion *configData); bool setupFrameFixedWing(QString airframeType); - bool setupFrameVtail(QString airframeType); + bool setupFrameElevon(QString airframeType); bool setupFixedWingMixer(double mixerFactors[8][3]); void setupMotors(QList motorList); From 28246cccbe3b362a61a8612b4c1405214cb52a9f Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sat, 7 Jun 2014 23:04:12 -0400 Subject: [PATCH 030/718] fix remaining headers --- .../src/plugins/setupwizard/connectiondiagram.cpp | 4 ++-- .../src/plugins/setupwizard/setupwizard.cpp | 4 ++-- .../setupwizard/vehicleconfigurationhelper.cpp | 14 +++++++------- .../setupwizard/vehicleconfigurationhelper.h | 2 +- .../setupwizard/vehicleconfigurationsource.h | 2 +- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 7525d5e55..6057098b1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -122,8 +122,8 @@ void ConnectionDiagram::setupGraphicsScene() case VehicleConfigurationSource::FIXED_WING_AILERON: elementsToShow << "aileron"; break; - case VehicleConfigurationSource::FIXED_WING_VTAIL: - elementsToShow << "vtail"; + case VehicleConfigurationSource::FIXED_WING_ELEVON: + elementsToShow << "elevon"; break; default: break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 449296f5b..09be733f1 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -236,8 +236,8 @@ QString SetupWizard::getSummaryText() case SetupWizard::FIXED_WING_AILERON: summary.append(tr("Aileron")); break; - case SetupWizard::FIXED_WING_VTAIL: - summary.append(tr("Vtail")); + case SetupWizard::FIXED_WING_ELEVON: + summary.append(tr("Elevon")); break; default: summary.append(tr("Unknown")); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index d5cf4ad5e..97219ada9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -221,8 +221,8 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() case VehicleConfigurationSource::FIXED_WING_AILERON: setupAileron(); break; - case VehicleConfigurationSource::FIXED_WING_VTAIL: - setupVtail(); + case VehicleConfigurationSource::FIXED_WING_ELEVON: + setupElevon(); break; default: break; @@ -1332,7 +1332,7 @@ void VehicleConfigurationHelper::setupOctoCopter() applyMultiGUISettings(frame, guiSettings); } -void VehicleConfigurationHelper::setupVtail() +void VehicleConfigurationHelper::setupElevon() { mixerChannelSettings channels[10]; @@ -1348,15 +1348,15 @@ void VehicleConfigurationHelper::setupVtail() channels[1].type = MIXER_TYPE_SERVO; channels[1].throttle1 = 0; channels[1].throttle2 = 0; - channels[1].roll = -100; - channels[1].pitch = 50; + channels[1].roll = -127; + channels[1].pitch = 127; channels[1].yaw = 0; channels[2].type = MIXER_TYPE_SERVO; channels[2].throttle1 = 0; channels[2].throttle2 = 0; - channels[2].roll = 100; - channels[2].pitch = -50; + channels[2].roll = 127; + channels[2].pitch = -127; channels[2].yaw = 0; channels[3].type = MIXER_TYPE_SERVO; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index d3ef88df4..7d92bd4ce 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -103,7 +103,7 @@ private: void setupQuadCopter(); void setupHexaCopter(); void setupOctoCopter(); - void setupVtail(); + void setupElevon(); void setupAileron(); private slots: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 98f06a991..53e0f8736 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -60,7 +60,7 @@ public: 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_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 }; + FIXED_WING_ELEVON, 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 }; From 75585097203190fc8fa373c0bf597aa6e62d7d54 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Sun, 8 Jun 2014 00:00:43 -0400 Subject: [PATCH 031/718] Revert "Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties." This reverts commit 11fa414e1a9efd6c32f52d43ca04bd5fef5e5048. Conflicts: ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp --- .../src/plugins/config/images/fixedwing-shapes.svg | 14 +++++++++----- .../plugins/setupwizard/pages/fixedwingpage.cpp | 5 +++++ 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index e518dd1f3..f08814a7d 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -19,7 +19,7 @@ viewBox="0 0 792 2016" enable-background="new 0 0 792 1008" xml:space="preserve" - inkscape:version="0.48.2 r9819" + inkscape:version="0.48.4 r9939" sodipodi:docname="fixedwing-shapes.svg">image/svg+xml>>>>>> parent of 11fa414... Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties. transform="matrix(0.8,0,0,0.8,78.355285,170.6871)">>>>>>> parent of 11fa414... Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties. break; default: elementId = ""; From b9733363b360f3983d2465800ef154d4c5564ceb Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 12:23:33 -0400 Subject: [PATCH 032/718] some revision control data is misplaced preventing compile --- .../src/plugins/config/images/fixedwing-shapes.svg | 4 ---- .../src/plugins/setupwizard/pages/fixedwingpage.cpp | 5 ----- 2 files changed, 9 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg index f08814a7d..52d8f72b5 100644 --- a/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg +++ b/ground/openpilotgcs/src/plugins/config/images/fixedwing-shapes.svg @@ -1100,11 +1100,7 @@ id="path8516_1_" inkscape:connector-curvature="0" style="fill:none;stroke:#010101;stroke-width:0.69520003;stroke-linecap:round;stroke-linejoin:round" />>>>>>> parent of 11fa414... Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties. transform="matrix(0.8,0,0,0.8,78.355285,170.6871)">>>>>>> parent of 11fa414... Fix minor cosmetic issue where vtail model was not rendering in the Configuration screen. Edit the Inkscape Object Properties. break; default: elementId = ""; From 1f74a782631cff1e7d11c933be75fdf0351111dc Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 14:41:42 -0400 Subject: [PATCH 033/718] Attempt adding in the proper Mixer setting constructs for Elevon setup. --- .../configfixedwingwidget.cpp | 94 ++++++++++--------- .../cfg_vehicletypes/configfixedwingwidget.h | 2 + .../config/configvehicletypewidget.cpp | 6 +- .../plugins/setupwizard/connectiondiagram.cpp | 3 + .../setupwizard/pages/fixedwingpage.cpp | 3 + .../src/plugins/setupwizard/setupwizard.cpp | 3 + .../vehicleconfigurationhelper.cpp | 5 +- .../setupwizard/vehicleconfigurationhelper.h | 1 + .../setupwizard/vehicleconfigurationsource.h | 2 +- 9 files changed, 71 insertions(+), 48 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 16eb7fa90..21d7fce1b 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -94,7 +94,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : // Set default model to "Elevator aileron rudder" m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); - // setupUI(m_aircraft->fixedWingType->currentText()); + setupUI(m_aircraft->fixedWingType->currentText()); connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); updateEnableControls(); @@ -138,6 +138,8 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->elevonSlider1->setEnabled(false); m_aircraft->elevonSlider2->setEnabled(false); + } else if (frameType == "FixedWingVtail" || frameType == "Vtail") { + // do nothing for now } else if (frameType == "FixedWingElevon" || frameType == "Elevon") { plane->setElementId("elevon"); setComboCurrentIndex(m_aircraft->fixedWingType, m_aircraft->fixedWingType->findText("Elevon")); @@ -152,7 +154,7 @@ void ConfigFixedWingWidget::setupUI(QString frameType) m_aircraft->fwAileron1Label->setText("Aileron 1"); m_aircraft->fwAileron2Label->setText("Aileron 2"); - m_aircraft->elevonLabel1->setText("Rudder"); + m_aircraft->elevonLabel1->setText("Roll"); m_aircraft->elevonLabel2->setText("Pitch"); m_aircraft->elevonSlider1->setEnabled(true); @@ -182,7 +184,9 @@ void ConfigFixedWingWidget::setupEnabledControls(QString frameType) } } - if (frameType == "Elevon" || frameType == "elevon") { + if (frameType == "Vtail" || frameType == "vtail") { + // enableComboBoxes(this, CHANNELBOXNAME, 3, true); + } else if (frameType == "Elevon" || frameType == "Elevon") { enableComboBoxes(this, CHANNELBOXNAME, 3, true); } else if (frameType == "aileron" || frameType == "Elevator aileron rudder") { enableComboBoxes(this, CHANNELBOXNAME, 4, true); @@ -250,8 +254,6 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) setComboCurrentIndex(m_aircraft->fwAileron2ChannelBox, fixed.FixedWingRoll2); setComboCurrentIndex(m_aircraft->fwElevator1ChannelBox, fixed.FixedWingPitch1); setComboCurrentIndex(m_aircraft->fwElevator2ChannelBox, fixed.FixedWingPitch2); - setComboCurrentIndex(m_aircraft->fwRudder1ChannelBox, fixed.FixedWingYaw1); - setComboCurrentIndex(m_aircraft->fwRudder2ChannelBox, fixed.FixedWingYaw2); if (frameType == "FixedWingElevon") { // If the airframe is elevon, restore the slider setting @@ -260,7 +262,7 @@ void ConfigFixedWingWidget::refreshWidgetsValues(QString frameType) if (channel > -1) { // If for some reason the actuators were incoherent, we might fail here, hence the check. m_aircraft->elevonSlider1->setValue( - getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW) * 100); + getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL) * 100); m_aircraft->elevonSlider2->setValue( getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH) * 100); } @@ -319,7 +321,7 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() { 0, 0, 0 }, { 0, 0, 0 } }; - setupFixedWingMixer(mixerMatrix); + setupFixedWingElevonMixer(mixerMatrix); m_aircraft->fwStatusLabel->setText(tr("Configuration OK")); @@ -483,74 +485,77 @@ bool ConfigFixedWingWidget::setupFrameElevon(QString airframeType) // 1. Assign the servo/motor/none for each channel - double value; - // motor int channel = m_aircraft->fwEngineChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_MOTOR); setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_THROTTLECURVE1, 127); - // rudders - channel = m_aircraft->fwRudder1ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, 127); - - channel = m_aircraft->fwRudder2ChannelBox->currentIndex() - 1; - setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW, -127); - // ailerons channel = m_aircraft->fwAileron1ChannelBox->currentIndex() - 1; if (channel > -1) { setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, value); - + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, 127); channel = m_aircraft->fwAileron2ChannelBox->currentIndex() - 1; setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); - value = (double)(m_aircraft->elevonSlider2->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, value); - value = (double)(m_aircraft->elevonSlider1->value() * 1.27); - setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -value); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, -127); + } + + // elevon + channel = m_aircraft->fwElevator1ChannelBox->currentIndex() - 1; + if (channel > -1) { + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, 127); + + channel = m_aircraft->fwElevator2ChannelBox->currentIndex() - 1; + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, -127); } m_aircraft->fwStatusLabel->setText("Mixer generated"); return true; + } /** - This function sets up the elevon fixed wing mixer values. + Helper function: setupElevonServo */ -bool ConfigFixedWingWidget::setupFixedWingMixer(double mixerFactors[8][3]) +void ConfigFixedWingWidget::setupElevonServo(int channel, double pitch, double roll) { UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); + + setMixerType(mixer, channel, VehicleConfig::MIXERTYPE_SERVO); + + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL, roll * 127); + setMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_PITCH, pitch * 127); +} + +/** + This function sets up the elevon fixed wing mixer values. + */ +bool ConfigFixedWingWidget::setupFixedWingElevonMixer(double mixerFactors[8][3]){ + UAVDataObject *mixer = dynamic_cast(getObjectManager()->getObject(QString("MixerSettings"))); + Q_ASSERT(mixer); resetMotorAndServoMixers(mixer); // and enable only the relevant channels: -// double pFactor = (double)m_aircraft->fwPitchMixLevel->value() / 100.0; -// double rFactor = (double)m_aircraft->fwRollMixLevel->value() / 100.0; - //invertMotors = m_aircraft->MultirotorRevMixerCheckBox->isChecked(); -// double yFactor = (double)m_aircraft->fwYawMixLevel->value() / 100.0; + double pFactor = (double)m_aircraft->elevonSlider1->value() / 100.0; + double rFactor = (double)m_aircraft->elevonSlider2->value() / 100.0; QList mmList; mmList << m_aircraft->fwEngineChannelBox << m_aircraft->fwAileron1ChannelBox << m_aircraft->fwAileron2ChannelBox << m_aircraft->fwElevator1ChannelBox - << m_aircraft->fwElevator2ChannelBox << m_aircraft->fwRudder1ChannelBox - << m_aircraft->fwRudder2ChannelBox; + << m_aircraft->fwElevator2ChannelBox; for (int i = 0; i < 8; i++) { if (mmList.at(i)->isEnabled()) { int channel = mmList.at(i)->currentIndex() - 1; if (channel > -1) { - qDebug() << "code needs to be written here!"; -// setupQuadMotor(channel, mixerFactors[i][0] * pFactor, rFactor * mixerFactors[i][1], -// yFactor * mixerFactors[i][2]); + setupElevonServo(channel, mixerFactors[i][0] * pFactor, rFactor * mixerFactors[i][1]); } } } @@ -604,20 +609,21 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) m_aircraft->fwEngineChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - if (m_aircraft->fwAileron1ChannelBox->currentText() == "None") { - m_aircraft->fwAileron1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + if (m_aircraft->fwElevator1ChannelBox->currentText() == "None") { + m_aircraft->fwElevator1ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwElevator1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } - if (m_aircraft->fwAileron2ChannelBox->currentText() == "None") { - m_aircraft->fwAileron2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes + if (m_aircraft->fwElevator2ChannelBox->currentText() == "None") { + m_aircraft->fwElevator2ChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; } else { - m_aircraft->fwAileron2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes + m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } + if (error) { m_aircraft->fwStatusLabel->setText(QString("ERROR: Assign all necessary channels")); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index cd8fd4611..582420ff0 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -66,8 +66,10 @@ private: virtual void resetActuators(GUIConfigDataUnion *configData); bool setupFrameFixedWing(QString airframeType); + bool setupFrameVtail(QString airframeType); bool setupFrameElevon(QString airframeType); bool setupFixedWingMixer(double mixerFactors[8][3]); + bool setupFixedWingElevonMixer(double mixerFactors[8][3]); void setupMotors(QList motorList); void updateAirframe(QString multiRotorType); diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index 318d71051..f36f02550 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -71,10 +71,12 @@ QStringList ConfigVehicleTypeWidget::getChannelDescriptions() channelDesc = ConfigFixedWingWidget::getChannelDescriptions(); break; case SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON: - // do nothing for elevon support for the time being. - case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: channelDesc = ConfigFixedWingWidget::getChannelDescriptions(); break; + case SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL: + // do nothing for vtail support for the time being. + // channelDesc = ConfigFixedWingWidget::getChannelDescriptions(); + break; case SystemSettings::AIRFRAMETYPE_HELICP: // helicp channelDesc = ConfigCcpmWidget::getChannelDescriptions(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 6057098b1..ac0bedbad 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -125,6 +125,9 @@ void ConnectionDiagram::setupGraphicsScene() case VehicleConfigurationSource::FIXED_WING_ELEVON: elementsToShow << "elevon"; break; + case VehicleConfigurationSource::FIXED_WING_VTAIL: + elementsToShow << "vtail"; + break; default: break; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp index f2d8f27ab..cb832267a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.cpp @@ -108,6 +108,9 @@ void FixedWingPage::updateImageAndDescription() case SetupWizard::FIXED_WING_ELEVON: elementId = "elevon"; break; +// case SetupWizard::FIXED_WING_VTAIL: +// elementId = "vtail"; +// break; default: elementId = ""; break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 09be733f1..7fa6c8fa0 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -239,6 +239,9 @@ QString SetupWizard::getSummaryText() case SetupWizard::FIXED_WING_ELEVON: summary.append(tr("Elevon")); break; +// case SetupWizard::FIXED_WING_VTAIL: +// summary.append(tr("Vtail")); +// break; default: summary.append(tr("Unknown")); break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 97219ada9..4aa3364d8 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -224,6 +224,9 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() case VehicleConfigurationSource::FIXED_WING_ELEVON: setupElevon(); break; + case VehicleConfigurationSource::FIXED_WING_VTAIL: +// setupVtail(); + break; default: break; } @@ -1378,7 +1381,7 @@ void VehicleConfigurationHelper::setupElevon() guiSettings.fixedwing.FixedWingRoll2 = 3; applyMixerConfiguration(channels); - applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGVTAIL, guiSettings); + applyMultiGUISettings(SystemSettings::AIRFRAMETYPE_FIXEDWINGELEVON, guiSettings); } void VehicleConfigurationHelper::setupAileron() diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index 7d92bd4ce..7829ac039 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -103,6 +103,7 @@ private: void setupQuadCopter(); void setupHexaCopter(); void setupOctoCopter(); + void setupVtail(); void setupElevon(); void setupAileron(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 53e0f8736..8175b4334 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -60,7 +60,7 @@ public: 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_COAX_Y, MULTI_ROTOR_OCTO, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, - FIXED_WING_ELEVON, HELI_CCPM }; + FIXED_WING_VTAIL, FIXED_WING_ELEVON, 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 }; From 569209a5108f7e7dbcd0ceb4b01f5c6c2362dc3f Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 15:20:31 -0400 Subject: [PATCH 034/718] add setupElevonServo function --- .../src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h | 1 + 1 file changed, 1 insertion(+) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h index 582420ff0..67048fa53 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h @@ -71,6 +71,7 @@ private: bool setupFixedWingMixer(double mixerFactors[8][3]); bool setupFixedWingElevonMixer(double mixerFactors[8][3]); void setupMotors(QList motorList); + void setupElevonServo(int channel, double roll, double pitch); void updateAirframe(QString multiRotorType); void setupEnabledControls(QString airframeType); From 64b2fce3d40a397778364a7291871ff17d84d658 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 16:02:15 -0400 Subject: [PATCH 035/718] I created some sort of issue in the Output warning highlights. Disable for now. --- .../plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 21d7fce1b..9dad31617 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -94,7 +94,7 @@ ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : // Set default model to "Elevator aileron rudder" m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); - setupUI(m_aircraft->fixedWingType->currentText()); + //setupUI(m_aircraft->fixedWingType->currentText()); connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); updateEnableControls(); @@ -602,6 +602,7 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) m_aircraft->fwRudder1ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } } else if (airframeType == "FixedWingElevon") { +/* if (m_aircraft->fwEngineChannelBox->currentText() == "None") { m_aircraft->fwEngineChannelBox->setItemData(0, pixmap, Qt::DecorationRole); // Set color palettes error = true; @@ -622,6 +623,7 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) } else { m_aircraft->fwElevator2ChannelBox->setItemData(0, 0, Qt::DecorationRole); // Reset color palettes } +*/ } if (error) { From 47673f519e7f5730fde42b8d378f142e06b7f1c1 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 21:25:05 -0400 Subject: [PATCH 036/718] Attempt to set some mixer values --- .../plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 9dad31617..c79a91007 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -313,8 +313,8 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() // pitch roll yaw double mixerMatrix[8][3] = { { 0, 0, 0 }, - { 0, 0, 0 }, - { 0, 0, 0 }, + { 127, -127, 0 }, + { -127, 127, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, From 2dcc9c59f1b207f566ea1702ae9131816af1f773 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Wed, 11 Jun 2014 21:28:20 -0400 Subject: [PATCH 037/718] correct values to be more appropriate for VehicleConfigurationHelper::applyMixerConfiguration() --- .../plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index c79a91007..2fc27cd51 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -313,8 +313,8 @@ QString ConfigFixedWingWidget::updateConfigObjectsFromWidgets() // pitch roll yaw double mixerMatrix[8][3] = { { 0, 0, 0 }, - { 127, -127, 0 }, - { -127, 127, 0 }, + { 1, -1, 0 }, + { -1, 1, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }, From 1bf9f250dc2f38eea57edb94fe3320cc2211e4e0 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Thu, 12 Jun 2014 02:43:38 -0400 Subject: [PATCH 038/718] poor attempt to set some default PIDS? make a note to change the modelview ehre as well. --- .../vehicleconfigurationhelper.cpp | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 4aa3364d8..3968954ff 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -35,6 +35,7 @@ #include "manualcontrolsettings.h" #include "flightmodesettings.h" #include "stabilizationsettings.h" +#include "stabilizationsettingsbank1.h" #include "revocalibration.h" #include "accelgyrosettings.h" @@ -330,6 +331,29 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() qDebug() << "Save Fixed Wing Actuator Data\n"; actSettings->setData(data); addModifiedObject(actSettings, tr("Writing actuator settings")); + + // I think I need to add a saved stabilizationsettings object also. + // unfortunately not sure this is the right way to do it! + + qDebug() << "Save Fixed Wing Default PID Data\n"; + + StabilizationSettingsBank1 *stabilizationSettingsBank = StabilizationSettingsBank1::GetInstance(m_uavoManager); + StabilizationSettingsBank1::DataFields stabSettingsBank = stabilizationSettingsBank->getData(); + stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KP] = 666; + stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KI] = 666; + stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KP] = 666; + stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KI] = 666; + stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KP] = 666; + stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KI] = 666; + stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KP] = 666; + stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KI] = 666; + + addModifiedObject(stabilizationSettingsBank, tr("Writing stabilization bank 1 settings")); + + // Set up model view image here? + // loop through all the window instances and check which are of the type ModelViewGadget. + // per m_thread on each instance setAcFileName(QString model_file_name) and then call reloadScene() on the same object. + break; } From 9aaa764797ef1356924918e5ff62888b7cfbd0f3 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Thu, 12 Jun 2014 11:05:46 -0400 Subject: [PATCH 039/718] work out a method to set some default PIDs... using a setData() call helps!!! --- .../vehicleconfigurationhelper.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 3968954ff..287727454 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -335,19 +335,30 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() // I think I need to add a saved stabilizationsettings object also. // unfortunately not sure this is the right way to do it! - qDebug() << "Save Fixed Wing Default PID Data\n"; + qDebug() << "Saving Fixed Wing Default PID Data\n"; +// qDebug() << "Save Fixed Wing StabilizationSettings object\n"; +// StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(m_uavoManager); +// StabilizationSettings::DataFields stabSettings = stabilizationSettings->getData(); +// Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettings.h +// stabSettings.CruiseControlInvertedThrustReversing[StabilizationSettings::CRUISECONTROLINVERTEDTHRUSTREVERSING_UNREVERSED] = 0; +// Currently broken, need to set max servo throw via iLimit here per my notes in http://forums.openpilot.org/topic/32356-gonna-give-revo-a-run-on-my-bixler-v1-any-suggestions/#entry252556 +// stabilizationSettings->setData(stabSettings); +// addModifiedObject(stabilizationSettings, tr("Writing stabilization settings")); + qDebug() << "Save Fixed Wing StabilizationSettingsBank1 object\n"; + +// Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettingsbank1.h StabilizationSettingsBank1 *stabilizationSettingsBank = StabilizationSettingsBank1::GetInstance(m_uavoManager); StabilizationSettingsBank1::DataFields stabSettingsBank = stabilizationSettingsBank->getData(); - stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KP] = 666; - stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KI] = 666; - stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KP] = 666; - stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KI] = 666; - stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KP] = 666; - stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KI] = 666; - stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KP] = 666; - stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KI] = 666; - + stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KP] = 2.420; + stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KI] = 2.420; + stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KP] = 2.420; + stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KI] = 2.420; + stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KP] = 2.420; + stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KI] = 2.420; + stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KP] = 2.420; + stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KI] = 2.420; + stabilizationSettingsBank->setData(stabSettingsBank); addModifiedObject(stabilizationSettingsBank, tr("Writing stabilization bank 1 settings")); // Set up model view image here? From 58dad5d3a5a87171edbad4ca42a2a01b53400ded Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Thu, 12 Jun 2014 12:07:03 -0400 Subject: [PATCH 040/718] Try to set Ilimit Rate... somehow I set Ilimit Attitude instead. Need to sort out. basic idea worked though. --- .../vehicleconfigurationhelper.cpp | 34 ++++++++++++++----- 1 file changed, 25 insertions(+), 9 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 287727454..d022839d6 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -35,6 +35,7 @@ #include "manualcontrolsettings.h" #include "flightmodesettings.h" #include "stabilizationsettings.h" +#include "stabilizationbank.h" #include "stabilizationsettingsbank1.h" #include "revocalibration.h" #include "accelgyrosettings.h" @@ -336,28 +337,43 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() // unfortunately not sure this is the right way to do it! qDebug() << "Saving Fixed Wing Default PID Data\n"; -// qDebug() << "Save Fixed Wing StabilizationSettings object\n"; -// StabilizationSettings *stabilizationSettings = StabilizationSettings::GetInstance(m_uavoManager); -// StabilizationSettings::DataFields stabSettings = stabilizationSettings->getData(); -// Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettings.h -// stabSettings.CruiseControlInvertedThrustReversing[StabilizationSettings::CRUISECONTROLINVERTEDTHRUSTREVERSING_UNREVERSED] = 0; -// Currently broken, need to set max servo throw via iLimit here per my notes in http://forums.openpilot.org/topic/32356-gonna-give-revo-a-run-on-my-bixler-v1-any-suggestions/#entry252556 -// stabilizationSettings->setData(stabSettings); -// addModifiedObject(stabilizationSettings, tr("Writing stabilization settings")); + +/* + qDebug() << "Save Fixed Wing StabilizationBank object\n"; + StabilizationBank *stabilizationBank = StabilizationBank::GetInstance(m_uavoManager); + StabilizationBank::DataFields stabBank = stabilizationBank->getData(); + Values pulled from ./build/uavobject-synthetics/gcs/stabilizationbank.h + stabBank.PitchPI[StabilizationBank::PITCHPI_ILIMIT] = 1.123; + stabilizationBank->setData(stabBank); + addModifiedObject(stabilizationBank, tr("Writing stabilization bank")); +*/ qDebug() << "Save Fixed Wing StabilizationSettingsBank1 object\n"; -// Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettingsbank1.h + // Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettingsbank1.h StabilizationSettingsBank1 *stabilizationSettingsBank = StabilizationSettingsBank1::GetInstance(m_uavoManager); StabilizationSettingsBank1::DataFields stabSettingsBank = stabilizationSettingsBank->getData(); stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KP] = 2.420; stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KI] = 2.420; stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KP] = 2.420; stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KI] = 2.420; + stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KP] = 2.420; stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KI] = 2.420; stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KP] = 2.420; stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KI] = 2.420; + + stabSettingsBank.YawRatePID[StabilizationSettingsBank1::YAWRATEPID_KP] = 2.420; + stabSettingsBank.YawRatePID[StabilizationSettingsBank1::YAWRATEPID_KI] = 2.420; + stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_KP] = 2.420; + stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_KI] = 2.420; + + + // need to set max servo throw via iLimit here per my notes in http://forums.openpilot.org/topic/32356-gonna-give-revo-a-run-on-my-bixler-v1-any-suggestions/#entry252556 + stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_ILIMIT] = 1.420; + stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_ILIMIT] = 1.420; + stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_ILIMIT] = 1.420; + stabilizationSettingsBank->setData(stabSettingsBank); addModifiedObject(stabilizationSettingsBank, tr("Writing stabilization bank 1 settings")); From 3d9f1ca93a38f6c2b50bca7ac14f407be30a034a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Jun 2014 18:51:26 +0200 Subject: [PATCH 041/718] OP-1317 changed imu wind estimation to actually estimate wind (and lowpass filter it) --- flight/modules/Airspeed/imu_airspeed.c | 65 +++++++++++++++----------- 1 file changed, 39 insertions(+), 26 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 9c5ffd789..17a1c74bf 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -55,8 +55,10 @@ struct IMUGlobals { float v1n1, v1n2; float v2n1, v2n2; float v3n1, v3n2; - float Vn1, Vn2; - + float Vw1n1, Vw1n2; + float Vw2n1, Vw2n2; + float Vw3n1, Vw3n2; + float Vw1, Vw2, Vw3; // storage variables for derivative calculation float pOld, yOld; @@ -115,13 +117,13 @@ static void PY2xB(const float p, const float y, float x[3]) } -static void PY2DeltaxB(const float p, const float y, float x[3]) +static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[3]) { const float cosp = cosf(p); - x[0] -= cosp * cosf(y); - x[1] -= cosp * sinf(y); - x[2] -= -sinf(p); + x[0] = xB[0] - cosp * cosf(y); + x[1] = xB[1] - cosp * sinf(y); + x[2] = xB[2] - -sinf(p); } @@ -163,15 +165,18 @@ void imu_airspeedInitialize() // get pitch and yaw from quarternion; principal argument for yaw Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true); - imu->pn1 = imu->pn2 = imu->pOld; - imu->yn1 = imu->yn2 = imu->yOld; + imu->pn1 = imu->pn2 = imu->pOld; + imu->yn1 = imu->yn2 = imu->yOld; - imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North; - imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East; - imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down; + imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North; + imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East; + imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down; - // initial guess for airspeed is modulus of groundspeed - imu->Vn1 = imu->Vn2 = sqrt(Sq(velData.North) + Sq(velData.East) + Sq(velData.Down)); + // initial guess for windspeed is zero + imu->Vw1n1 = imu->Vw1n2 = 0.0f; + imu->Vw2n1 = imu->Vw2n2 = 0.0f; + imu->Vw3n1 = imu->Vw3n2 = 0.0f; + imu->Vw1 = imu->Vw2 = 0.0f; } /* @@ -202,6 +207,7 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air float normDiffAttitude2; float dvdtDotdfdt; + float xB[3]; // get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed { // Scoping to save memory AttitudeStateData attData; @@ -219,10 +225,10 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air p = FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); y = FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); // transform pitch and yaw into fuselage vector xB and xBold - PY2xB(p, y, dxB); + PY2xB(p, y, xB); // calculate change in fuselage vector by substraction of old value - PY2DeltaxB(imu->pOld, imu->yOld, dxB); - + PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB); + // filter ground speed from VelocityState const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); @@ -250,17 +256,24 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air // Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2 // airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt const float airspeed = dvdtDotdfdt / normDiffAttitude2; - // filter raw airspeed - const float fVn = FilterButterWorthDF2(ffV, airspeed, &(imu->Vn1), &(imu->Vn2)); - airspeedData->CalibratedAirspeed = fVn; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK); - } else { - airspeedData->CalibratedAirspeed = 0; - airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; - AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING); - } + // groundspeed = airspeed + wind ---> wind = groundspeed - airspeed + const float wind[3] = { imu->v1Old - xB[0] * airspeed, + imu->v2Old - xB[1] * airspeed, + imu->v3Old - xB[2] * airspeed }; + // filter raw wind + imu->Vw1 = FilterButterWorthDF2(ffV, wind[0], &(imu->Vw1n1), &(imu->Vw1n2)); + imu->Vw2 = FilterButterWorthDF2(ffV, wind[1], &(imu->Vw2n1), &(imu->Vw2n2)); + imu->Vw3 = FilterButterWorthDF2(ffV, wind[2], &(imu->Vw3n1), &(imu->Vw3n2)); + } // else leave wind estimation unchanged + + // airspeed = groundspeed - wind + airspeedData->CalibratedAirspeed = sqrtf( + (imu->v1Old - imu->Vw1) * (imu->v1Old - imu->Vw1) + + (imu->v2Old - imu->Vw2) * (imu->v2Old - imu->Vw2) + + (imu->v3Old - imu->Vw3) * (imu->v3Old - imu->Vw3)); + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED); } From 7933f2154c65d43c6d995cbc082a4e949a037772 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 18 Jun 2014 12:44:10 +0200 Subject: [PATCH 042/718] OP-1317 - project 3d airspeed vector unto fuselage vector to get scalar airspeed estimate --- flight/modules/Airspeed/imu_airspeed.c | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 17a1c74bf..b17161b20 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -267,11 +267,18 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air imu->Vw3 = FilterButterWorthDF2(ffV, wind[2], &(imu->Vw3n1), &(imu->Vw3n2)); } // else leave wind estimation unchanged - // airspeed = groundspeed - wind - airspeedData->CalibratedAirspeed = sqrtf( - (imu->v1Old - imu->Vw1) * (imu->v1Old - imu->Vw1) - + (imu->v2Old - imu->Vw2) * (imu->v2Old - imu->Vw2) - + (imu->v3Old - imu->Vw3) * (imu->v3Old - imu->Vw3)); + { // Scoping to save memory + // airspeed = groundspeed - wind + const float Vair[3] = { + imu->v1Old - imu->Vw1, + imu->v2Old - imu->Vw2, + imu->v3Old - imu->Vw3 + }; + + // project airspeed into fuselage vector + airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2]; + } + airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED); } From 7245ea9267f23ff09b5bfdfcac563940eac2b5ca Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 18 Jun 2014 20:36:50 +0200 Subject: [PATCH 043/718] OP-1317 changed type and default for airspeedsettings filter coefficients --- shared/uavobjectdefinition/airspeedsettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index 00cd56241..8f52ac9fa 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -5,8 +5,8 @@ - - + + From 7898d507caa44cdb73cacba8e69acea3fc8bb5e7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 18 Jun 2014 20:41:58 +0200 Subject: [PATCH 044/718] drive-by bugfix in #ifdefed code affecting simposix --- flight/modules/Airspeed/airspeed.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 7111bc5bd..b01f14e7a 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -98,7 +98,7 @@ int32_t AirspeedInitialize() HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesArrayGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) { From d43797628762c75e35d2449ff29623e3377b33f7 Mon Sep 17 00:00:00 2001 From: James Duley Date: Thu, 3 Jul 2014 19:40:41 +1200 Subject: [PATCH 045/718] removed so version number from linux icu libs --- ground/openpilotgcs/copydata.pro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index a60b7b8a6..2805a26f6 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,9 +30,9 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ - libicui18n.so.51 \ - libicuuc.so.51 \ - libicudata.so.51 \ + libicui18n.so \ + libicuuc.so \ + libicudata.so \ libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() From 455c33fb29e9faf1fb054a4a290c626020e76b2a Mon Sep 17 00:00:00 2001 From: James Duley Date: Tue, 6 May 2014 15:22:44 +1200 Subject: [PATCH 046/718] check whether python is python 2 use python2 if not --- ground/openpilotgcs/src/python.pri | 2 +- make/tools.mk | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/python.pri b/ground/openpilotgcs/src/python.pri index 739fc2cc9..62d03795f 100644 --- a/ground/openpilotgcs/src/python.pri +++ b/ground/openpilotgcs/src/python.pri @@ -15,7 +15,7 @@ OPENPILOT_TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) PYTHON = \"$$ROOT_DIR/tools/$$PYTHON_DIR/python\" } else { # not found, hope it's in the path... - PYTHON = \"python\" + PYTHON = $$(PYTHON) } } diff --git a/make/tools.mk b/make/tools.mk index 5771abf28..d757091da 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -597,7 +597,11 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists) else # not installed, hope it's in the path... # $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH) - export PYTHON := python + ifeq ($(findstring Python 2,$(shell python --version)), Python 2) + export PYTHON := python + else + export PYTHON := python2 + endif endif .PHONY: python_version From 90b84b210e616d2ec786875da9960e21a6b752dd Mon Sep 17 00:00:00 2001 From: James Duley Date: Sat, 5 Jul 2014 23:30:40 +1200 Subject: [PATCH 047/718] Revert "removed so version number from linux icu libs" it breaks the build with qt_sdk_install This reverts commit d43797628762c75e35d2449ff29623e3377b33f7. --- ground/openpilotgcs/copydata.pro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 2805a26f6..a60b7b8a6 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,9 +30,9 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ - libicui18n.so \ - libicuuc.so \ - libicudata.so \ + libicui18n.so.51 \ + libicuuc.so.51 \ + libicudata.so.51 \ libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() From 95676043d7e7dbf78c3fbab0beb1bb09c24d16e5 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 6 Jul 2014 18:55:54 +1200 Subject: [PATCH 048/718] on linux check whether QT_INSTALL_LIBS are /usr/lib/* set copyqt to 0 if that is the case --- ground/openpilotgcs/copydata.pro | 2 +- ground/openpilotgcs/openpilotgcs.pri | 11 ++++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index a60b7b8a6..b277b0f95 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -3,7 +3,7 @@ include(openpilotgcs.pri) TEMPLATE = subdirs # Copy Qt runtime libraries into the build directory (to run or package) -equals(copydata, 1) { +equals(copyqt, 1) { GCS_LIBRARY_PATH diff --git a/ground/openpilotgcs/openpilotgcs.pri b/ground/openpilotgcs/openpilotgcs.pri index 31c0ed454..d8c392b9c 100644 --- a/ground/openpilotgcs/openpilotgcs.pri +++ b/ground/openpilotgcs/openpilotgcs.pri @@ -83,16 +83,26 @@ macx { GCS_DATA_BASENAME = Resources GCS_DOC_PATH = $$GCS_DATA_PATH/doc copydata = 1 + copyqt = 1 } else { + !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 win32 { contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1 GCS_APP_TARGET = openpilotgcs + copyqt = $$copydata } else { GCS_APP_WRAPPER = openpilotgcs GCS_APP_TARGET = openpilotgcs.bin GCS_QT_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5 GCS_QT_PLUGINS_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/plugins GCS_QT_QML_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/qt5/qml + lib_dir = $$[QT_INSTALL_LIBS] + lib_dir ~= s,/usr/lib/*,/usr/lib + equals(lib_dir, "/usr/lib") { + copyqt = 0 + } else { + copyqt = 1 + } } GCS_LIBRARY_PATH = $$GCS_BUILD_TREE/$$GCS_LIBRARY_BASENAME/openpilotgcs GCS_PLUGIN_PATH = $$GCS_LIBRARY_PATH/plugins @@ -100,7 +110,6 @@ macx { GCS_DATA_PATH = $$GCS_BUILD_TREE/share/openpilotgcs GCS_DATA_BASENAME = share/openpilotgcs GCS_DOC_PATH = $$GCS_BUILD_TREE/share/doc - !isEqual(GCS_SOURCE_TREE, $$GCS_BUILD_TREE):copydata = 1 } From 55b325966a185a4e63dfe3a19024220a2e1107bf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:47:09 +0200 Subject: [PATCH 049/718] OP-1395 - Pack several objects on a single Debug Log "slot", after ten consecutive failures (log full) gives up trying to save to prevent wasting time. Use PIOS_Delay for timestamp --- flight/pios/common/pios_debuglog.c | 125 ++++++++++++++----- shared/uavobjectdefinition/debuglogentry.xml | 4 +- 2 files changed, 93 insertions(+), 36 deletions(-) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 97cb1d462..05681fc79 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -49,6 +49,9 @@ static xSemaphoreHandle mutex = 0; #endif static bool logging_enabled = false; +#define MAX_CONSECUTIVE_FAILS_COUNT 10 +static bool log_is_full = false; +static uint8_t fails_count = 0; static uint16_t flightnum = 0; static uint16_t lognum = 0; static DebugLogEntryData *buffer = 0; @@ -56,8 +59,16 @@ static DebugLogEntryData *buffer = 0; static DebugLogEntryData staticbuffer; #endif -/* Private Function Prototypes */ +#define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data)) +#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE) +// build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number +#define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF)) +static uint32_t used_buffer_space = 0; + +/* Private Function Prototypes */ +static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data); +static bool write_current_buffer(); /** * @brief Initialize the log facility */ @@ -75,9 +86,12 @@ void PIOS_DEBUGLOG_Initialize() return; } mutexlock(); - lognum = 0; - flightnum = 0; - while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + lognum = 0; + flightnum = 0; + fails_count = 0; + used_buffer_space = 0; + log_is_full = false; + while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { flightnum++; } mutexunlock(); @@ -103,30 +117,13 @@ void PIOS_DEBUGLOG_Enable(uint8_t enabled) */ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data) { - if (!logging_enabled || !buffer) { + if (!logging_enabled || !buffer || log_is_full) { return; } mutexlock(); - buffer->Flight = flightnum; -#if defined(PIOS_INCLUDE_FREERTOS) - buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; -#else - buffer->FlightTime = 0; -#endif - buffer->Entry = lognum; - buffer->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT; - buffer->ObjectID = objid; - buffer->InstanceID = instid; - if (size > sizeof(buffer->Data)) { - size = sizeof(buffer->Data); - } - buffer->Size = size; - memset(buffer->Data, 0xff, sizeof(buffer->Data)); - memcpy(buffer->Data, data, size); - if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { - lognum++; - } + enqueue_data(objid, instid, size, data); + mutexunlock(); } /** @@ -137,27 +134,30 @@ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8 */ void PIOS_DEBUGLOG_Printf(char *format, ...) { - if (!logging_enabled || !buffer) { + if (!logging_enabled || !buffer || log_is_full) { return; } + va_list args; va_start(args, format); mutexlock(); + // flush any pending buffer before writing debug text + if (used_buffer_space) { + write_current_buffer(); + } memset(buffer->Data, 0xff, sizeof(buffer->Data)); vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args); buffer->Flight = flightnum; -#if defined(PIOS_INCLUDE_FREERTOS) - buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; -#else - buffer->FlightTime = 0; -#endif + + buffer->FlightTime = PIOS_DELAY_GetuS(); + buffer->Entry = lognum; buffer->Type = DEBUGLOGENTRY_TYPE_TEXT; buffer->ObjectID = 0; buffer->InstanceID = 0; buffer->Size = strlen((const char *)buffer->Data); - if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { lognum++; } mutexunlock(); @@ -179,7 +179,7 @@ void PIOS_DEBUGLOG_Printf(char *format, ...) int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst) { PIOS_Assert(mybuffer); - return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flight * 256, inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData)); + return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData)); } /** @@ -214,11 +214,68 @@ void PIOS_DEBUGLOG_Format(void) { mutexlock(); PIOS_FLASHFS_Format(pios_user_fs_id); - lognum = 0; - flightnum = 0; + lognum = 0; + flightnum = 0; + log_is_full = false; + fails_count = 0; + used_buffer_space = 0; mutexunlock(); } +void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data) +{ + DebugLogEntryData *entry; + + // start a new block + if (!used_buffer_space) { + entry = buffer; + memset(buffer->Data, 0xff, sizeof(buffer->Data)); + used_buffer_space += size; + } else { + // if an instance is being filled and there is enough space, does enqueues new data. + if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) { + buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS; + if (!write_current_buffer()) { + return; + } + entry = buffer; + memset(buffer->Data, 0xff, sizeof(buffer->Data)); + used_buffer_space += size; + } else { + entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space]; + used_buffer_space += size + LOG_ENTRY_HEADER_SIZE; + } + } + + entry->Flight = flightnum; + entry->FlightTime = PIOS_DELAY_GetuS(); + entry->Entry = lognum; + entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT; + entry->ObjectID = objid; + entry->InstanceID = instid; + if (size > sizeof(buffer->Data)) { + size = sizeof(buffer->Data); + } + entry->Size = size; + + memcpy(entry->Data, data, size); +} + +bool write_current_buffer() +{ + // not enough space, write the block and start a new one + if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + lognum++; + fails_count = 0; + used_buffer_space = 0; + } else { + if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) { + log_is_full = true; + } + return false; + } + return true; +} /** * @} * @} diff --git a/shared/uavobjectdefinition/debuglogentry.xml b/shared/uavobjectdefinition/debuglogentry.xml index 9a9acda83..8628fd73e 100644 --- a/shared/uavobjectdefinition/debuglogentry.xml +++ b/shared/uavobjectdefinition/debuglogentry.xml @@ -2,9 +2,9 @@ Log Entry in Flash - + - + From 80169e998f5b2dbfac6f2d8d867014dff488096f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:51:50 +0200 Subject: [PATCH 050/718] OP-1395 - Handles and unpack multiple uavos in single Debug Log Slot, default interval set to 500ms, --- .../src/plugins/flightlog/FlightLogDialog.qml | 15 ++++--- .../plugins/flightlog/flightlogmanager.cpp | 42 +++++++++++++++---- .../src/plugins/flightlog/flightlogmanager.h | 5 ++- .../src/plugins/flightlog/functions.js | 6 +++ 4 files changed, 54 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 9c3c730c7..115aa7b7f 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -7,7 +7,7 @@ import org.openpilot 1.0 import "functions.js" as Functions Rectangle { - width: 600 + width: 700 height: 400 id: root ColumnLayout { @@ -59,11 +59,11 @@ Rectangle { delegate: Text { verticalAlignment: Text.AlignVCenter - text: Functions.millisToTime(styleData.value) + text: Functions.microsToTime(styleData.value) } } TableViewColumn { - role: "Type"; title: "Type"; width: 60; + role: "Type"; title: "Type"; width: 50; delegate: Text { verticalAlignment: Text.AlignVCenter @@ -72,6 +72,7 @@ Rectangle { case 0 : text: qsTr("Empty"); break; case 1 : text: qsTr("Text"); break; case 2 : text: qsTr("UAVO"); break; + case 3 : text: qsTr("UAVO(P)"); break; default: text: qsTr("Unknown"); break; } } @@ -96,10 +97,14 @@ Rectangle { text: "" + qsTr("Flights recorded: ") + "" + (logStatus.Flight + 1) } Text { - id: totalEntries - text: "" + qsTr("Entries logged (free): ") + "" + + id: totalSlots + text: "" + qsTr("Slot used (free): ") + "" + logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" } + Text { + id: totalEntries + text: "" + qsTr("Entries logged: ") + "" + logManager.uavoEntries.count + } Rectangle { Layout.fillHeight: true } diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp index c0dd6f633..c43aa8fc3 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp @@ -205,21 +205,47 @@ void FlightLogManager::retrieveLogs(int flightToRetrieve) for (int flight = startFlight; flight <= endFlight; flight++) { m_flightLogControl->setFlight(flight); bool gotLast = false; - int entry = 0; + int slot = 0; while (!gotLast) { // Send request for loading flight entry on flight side and wait for ack/nack - m_flightLogControl->setEntry(entry); + m_flightLogControl->setEntry(slot); if (updateHelper.doObjectAndWait(m_flightLogControl, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS && requestHelper.doObjectAndWait(m_flightLogEntry, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS) { if (m_flightLogEntry->getType() != DebugLogEntry::TYPE_EMPTY) { // Ok, we retrieved the entry, and it was the correct one. clone it and add it to the list ExtendedDebugLogEntry *logEntry = new ExtendedDebugLogEntry(); + logEntry->setData(m_flightLogEntry->getData(), m_objectManager); m_logEntries << logEntry; + if (logEntry->getData().Type == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { + const quint32 total_len = sizeof(DebugLogEntry::DataFields); + const quint32 data_len = sizeof(((DebugLogEntry::DataFields *)0)->Data); + const quint32 header_len = total_len - data_len; + + DebugLogEntry::DataFields fields; + quint32 start = logEntry->getData().Size; + + // cycle until there is space for another object + while (start + header_len + 1 < data_len) { + memset(&fields, 0xFF, total_len); + memcpy(&fields, &logEntry->getData().Data[start], header_len); + // check wether a packed object is found + // note that empty data blocks are set as 0xFF in flight side to minimize flash wearing + // thus as soon as this read outside of used area, the test will fail as lenght would be 0xFFFF + quint32 toread = header_len + fields.Size; + if (!(toread + start > data_len)) { + memcpy(&fields, &logEntry->getData().Data[start], toread); + ExtendedDebugLogEntry *subEntry = new ExtendedDebugLogEntry(); + subEntry->setData(fields, m_objectManager); + m_logEntries << subEntry; + } + start += toread; + } + } // Increment to get next entry from flight side - entry++; + slot++; } else { // We are done, not more entries on this flight gotLast = true; @@ -280,7 +306,7 @@ void FlightLogManager::exportToOPL(QString fileName) ExtendedDebugLogEntry *entry = m_logEntries[currentEntry]; // Only log uavobjects - if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT) { + if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT || entry->getType() == ExtendedDebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { // Set timestamp that should be logged for this entry logFile.setNextTimeStamp(entry->getFlightTime() - adjustedBaseTime); @@ -615,7 +641,7 @@ QString ExtendedDebugLogEntry::getLogString() { if (getType() == DebugLogEntry::TYPE_TEXT) { return QString((const char *)getData().Data); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { return m_object->toString().replace("\n", " ").replace("\t", " "); } else { return ""; @@ -631,7 +657,7 @@ void ExtendedDebugLogEntry::toXML(QXmlStreamWriter *xmlWriter, quint32 baseTime) if (getType() == DebugLogEntry::TYPE_TEXT) { xmlWriter->writeAttribute("type", "text"); xmlWriter->writeTextElement("message", QString((const char *)getData().Data)); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { xmlWriter->writeAttribute("type", "uavobject"); m_object->toXML(xmlWriter); } @@ -644,7 +670,7 @@ void ExtendedDebugLogEntry::toCSV(QTextStream *csvStream, quint32 baseTime) if (getType() == DebugLogEntry::TYPE_TEXT) { data = QString((const char *)getData().Data); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { data = m_object->toString().replace("\n", "").replace("\t", ""); } *csvStream << QString::number(getFlight() + 1) << '\t' << QString::number(getFlightTime() - baseTime) << '\t' << QString::number(getEntry()) << '\t' << data << '\n'; @@ -654,7 +680,7 @@ void ExtendedDebugLogEntry::setData(const DebugLogEntry::DataFields &data, UAVOb { DebugLogEntry::setData(data); - if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { UAVDataObject *object = (UAVDataObject *)objectManager->getObject(getObjectID(), getInstanceID()); Q_ASSERT(object); m_object = object->clone(getInstanceID()); diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 7fcf4c4d9..58d62954e 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -94,6 +94,10 @@ public slots: setDirty(true); if (m_setting != 1 && m_setting != 3) { setPeriod(0); + } else { + if (!period()) { + setPeriod(500); + } } emit settingChanged(setting); } @@ -240,7 +244,6 @@ public: { return m_loggingEnabled; } - signals: void logEntriesChanged(); void flightEntriesChanged(); diff --git a/ground/openpilotgcs/src/plugins/flightlog/functions.js b/ground/openpilotgcs/src/plugins/flightlog/functions.js index 47884432b..83b5c9e82 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/functions.js +++ b/ground/openpilotgcs/src/plugins/flightlog/functions.js @@ -11,6 +11,12 @@ function millisToTime(ms) { return pad(hours, 2) + ":" + pad(minutes, 2) + ":" + pad(seconds, 2) + ":" + pad(msleft, 3); } + +function microsToTime(us) { + var ms = Math.floor(us / 1000); + return millisToTime(ms); +} + function pad(number, length) { var str = '' + number; while (str.length < length) { From 711c3609b65d3676b2b941dedeb1ce08b359ab4d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:53:25 +0200 Subject: [PATCH 051/718] OP-1395 - Add support for Fast read, explicitly set SPI clock speed to maximum allowed by all parts used --- flight/pios/common/pios_flash_jedec.c | 58 +++++++++++++--------- flight/pios/inc/pios_flash_jedec_catalog.h | 24 ++++++--- flight/pios/inc/pios_flash_jedec_priv.h | 12 +++-- 3 files changed, 58 insertions(+), 36 deletions(-) diff --git a/flight/pios/common/pios_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index 9b1ceeee2..74a768184 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -75,13 +75,16 @@ struct jedec_flash_dev { enum pios_jedec_dev_magic magic; }; +#define FLASH_FAST_PRESCALER PIOS_SPI_PRESCALER_2 +#define FLASH_PRESCALER PIOS_SPI_PRESCALER_4 + // ! Private functions static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev); static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void); static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev); -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast); static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev); @@ -166,12 +169,12 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t sla * @brief Claim the SPI bus for flash use and assert CS pin * @return 0 for sucess, -1 for failure to get semaphore */ -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev) +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast) { if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) { return -1; } - + PIOS_SPI_SetClockSpeed(flash_dev->spi_id, fast ? FLASH_FAST_PRESCALER : FLASH_PRESCALER); PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); flash_dev->claimed = true; @@ -209,7 +212,7 @@ static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -226,7 +229,7 @@ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -1; } @@ -247,7 +250,7 @@ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -2; } @@ -354,7 +357,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) return ret; } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -368,7 +371,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) // Keep polling when bus is busy too while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); + vTaskDelay(2); #endif } @@ -394,7 +397,7 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) return ret; } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -455,16 +458,14 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin if (((addr & 0xff) + len) > 0x100) { return -3; } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { return ret; } /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { PIOS_Flash_Jedec_ReleaseBus(flash_dev); return -1; @@ -486,7 +487,7 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin #else // Query status this way to prevent accel chip locking us out - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -1; } @@ -536,13 +537,12 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s if (((addr & 0xff) + len) > 0x100) { return -3; } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { return ret; } /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -582,17 +582,29 @@ static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { return -1; } - - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) { + bool fast_read = flash_dev->cfg->fast_read != 0; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, fast_read) == -1) { return -1; } - /* Execute read command and clock in address. Keep CS asserted */ - uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - - if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; + if (!fast_read) { + uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } + } else { + uint8_t cmdlen = flash_dev->cfg->fast_read_dummy_bytes + 4; + uint8_t out[cmdlen]; + memset(out, 0x0, cmdlen); + out[0] = flash_dev->cfg->fast_read; + out[1] = (addr >> 16) & 0xff; + out[2] = (addr >> 8) & 0xff; + out[3] = addr & 0xff; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, cmdlen, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } } /* Copy the transfer data to the buffer */ diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index cb15fa8b9..32e5a70c8 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -40,29 +40,37 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x20, .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // m25px16 .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x71, .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // w25x .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x30, .expect_capacity = 0x13, - .sector_erase = 0x20, - .chip_erase = 0x60 + .sector_erase = 0x20, + .chip_erase = 0x60, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // 25q16 .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x40, .expect_capacity = 0x15, - .sector_erase = 0x20, - .chip_erase = 0x60 + .sector_erase = 0x20, + .chip_erase = 0x60, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, } }; const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog); diff --git a/flight/pios/inc/pios_flash_jedec_priv.h b/flight/pios/inc/pios_flash_jedec_priv.h index fb021ee3c..6d1d3353d 100644 --- a/flight/pios/inc/pios_flash_jedec_priv.h +++ b/flight/pios/inc/pios_flash_jedec_priv.h @@ -40,11 +40,13 @@ extern const struct pios_flash_driver pios_jedec_flash_driver; #define JEDEC_MANUFACTURER_WINBOND 0xEF struct pios_flash_jedec_cfg { - uint8_t expect_manufacturer; - uint8_t expect_memorytype; - uint8_t expect_capacity; - uint32_t sector_erase; - uint32_t chip_erase; + uint8_t expect_manufacturer; + uint8_t expect_memorytype; + uint8_t expect_capacity; + uint8_t sector_erase; + uint8_t chip_erase; + uint8_t fast_read; + uint8_t fast_read_dummy_bytes; }; int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num); From 03d03cfb06260df258d6c4a94da5bc8839cdf9e1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:54:17 +0200 Subject: [PATCH 052/718] OP-1395 add a vTaskDelay when waiting for dma transfer to complete when using FreeRTOS. --- flight/pios/stm32f4xx/pios_spi.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index 09ad51f98..889b885a9 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -585,6 +585,9 @@ static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer /* Wait until all bytes have been transmitted/received */ while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { +#if defined(PIOS_INCLUDE_FREERTOS) + vTaskDelay(0); +#endif ; } From 61d26d7fd8a9947502c42d24c95f64a20cdd6e0a Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 08:39:32 +0200 Subject: [PATCH 053/718] op-1395 - automatically increment flight number after disabling log. --- flight/pios/common/pios_debuglog.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 05681fc79..4ac83a757 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -104,6 +104,10 @@ void PIOS_DEBUGLOG_Initialize() */ void PIOS_DEBUGLOG_Enable(uint8_t enabled) { + // increase the flight num as soon as logging is disabled + if (!logging_enabled && enabled) { + flightnum++; + } logging_enabled = enabled; } From dc11ff51b1e3048235339c3dc3df13c4b3ffdfb3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 08:40:31 +0200 Subject: [PATCH 054/718] OP-1395 - fix typo in Flight log dialog --- ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 115aa7b7f..494dc6ea2 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -98,7 +98,7 @@ Rectangle { } Text { id: totalSlots - text: "" + qsTr("Slot used (free): ") + "" + + text: "" + qsTr("Slots used (free): ") + "" + logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" } Text { From e53be4f8ec54b2864edc76bac5ec2f1a2315dfc1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 19:49:14 +0200 Subject: [PATCH 055/718] OP-1395 - Fix log entries downloaded counter in dialog --- .../openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml | 2 +- ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 494dc6ea2..18b842159 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -103,7 +103,7 @@ Rectangle { } Text { id: totalEntries - text: "" + qsTr("Entries logged: ") + "" + logManager.uavoEntries.count + text: "" + qsTr("Entries downloaded: ") + "" + logManager.logEntriesCount } Rectangle { Layout.fillHeight: true diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 58d62954e..80147b1ab 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -182,7 +182,7 @@ class FlightLogManager : public QObject { Q_PROPERTY(QStringList logSettings READ logSettings NOTIFY logSettingsChanged) Q_PROPERTY(QStringList logStatuses READ logStatuses NOTIFY logStatusesChanged) Q_PROPERTY(int loggingEnabled READ loggingEnabled WRITE setLoggingEnabled NOTIFY loggingEnabledChanged) - + Q_PROPERTY(int logEntriesCount READ logEntriesCount NOTIFY logEntriesChanged) public: explicit FlightLogManager(QObject *parent = 0); @@ -244,6 +244,9 @@ public: { return m_loggingEnabled; } + int logEntriesCount(){ + return m_logEntries.count(); + } signals: void logEntriesChanged(); void flightEntriesChanged(); From 6c38eda19227161c417c29e882968669c20edff3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 23:05:38 +0200 Subject: [PATCH 056/718] OP-1395 - Reset log num when changing flight num. fix flight num update logic --- flight/pios/common/pios_debuglog.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 4ac83a757..f4549a399 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -105,8 +105,9 @@ void PIOS_DEBUGLOG_Initialize() void PIOS_DEBUGLOG_Enable(uint8_t enabled) { // increase the flight num as soon as logging is disabled - if (!logging_enabled && enabled) { + if (logging_enabled && !enabled) { flightnum++; + lognum = 0; } logging_enabled = enabled; } From c01047920adb9ff869681fa84fcdd4a74f00e79d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 23:06:14 +0200 Subject: [PATCH 057/718] OP-1395 - Remove Qt::Dialog flag as it is misbehaving in linux --- ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp b/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp index 07c5df7bf..68b043d86 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp @@ -86,7 +86,6 @@ void FlightLogPlugin::ShowLogManagementDialog() m_logDialog->rootContext()->setContextProperty("logDialog", m_logDialog); m_logDialog->setResizeMode(QQuickView::SizeRootObjectToView); m_logDialog->setSource(QUrl("qrc:/flightlog/FlightLogDialog.qml")); - m_logDialog->setFlags(Qt::Dialog); m_logDialog->setModality(Qt::ApplicationModal); connect(m_logDialog, SIGNAL(destroyed()), this, SLOT(LogManagementDialogClosed())); } From ca12105527ace72bc7397bb0d4b8493f48154d93 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 10 Jul 2014 22:05:12 +0200 Subject: [PATCH 058/718] OP-1395 - missing uncrustification --- ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 80147b1ab..0c93d014b 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -244,7 +244,8 @@ public: { return m_loggingEnabled; } - int logEntriesCount(){ + int logEntriesCount() + { return m_logEntries.count(); } signals: From fd307479e810b4ab006a825eaae716a734f3dbd1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 10 Jul 2014 22:12:25 +0200 Subject: [PATCH 059/718] OP-1395 - Change labels for better readability remove trailing spaces from translated strings --- .../src/plugins/flightlog/FlightLogDialog.qml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 18b842159..a24a28551 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -94,16 +94,16 @@ Rectangle { spacing: 10 Text { id: totalFlights - text: "" + qsTr("Flights recorded: ") + "" + (logStatus.Flight + 1) + text: "" + qsTr("Flights recorded:") + " " + (logStatus.Flight + 1) } Text { id: totalSlots - text: "" + qsTr("Slots used (free): ") + "" + - logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" + text: "" + qsTr("Slots used/free:") + " " + + logStatus.UsedSlots + "/" + logStatus.FreeSlots } Text { id: totalEntries - text: "" + qsTr("Entries downloaded: ") + "" + logManager.logEntriesCount + text: "" + qsTr("Entries downloaded:") + " " + logManager.logEntriesCount } Rectangle { Layout.fillHeight: true From 5ee35cdd753928e709bc4dea3b860817a1f6bec8 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 14 Jul 2014 18:19:12 +0200 Subject: [PATCH 060/718] OP-1398 OP-1399 Windows Qt 5.3.1 upgrade - upgraded qt_sdk_install to install Qt 5.3.1 --- make/tools.mk | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index cddb119a2..8d343d0af 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -82,9 +82,9 @@ else ifeq ($(UNAME), Darwin) else ifeq ($(UNAME), Windows) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip/+md5 - QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe - QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe.md5 - QT_SDK_ARCH := mingw48_32 + QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe + QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe.md5 + QT_SDK_ARCH := mingw482_32 NSIS_URL := http://wiki.openpilot.org/download/attachments/18612236/nsis-2.46-unicode.tar.bz2 SDL_URL := http://wiki.openpilot.org/download/attachments/18612236/SDL-devel-1.2.15-mingw32.tar.gz OPENSSL_URL := http://wiki.openpilot.org/download/attachments/18612236/openssl-1.0.1e-win32.tar.bz2 @@ -97,16 +97,19 @@ GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0 # Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1 -QT_SDK_DIR := $(TOOLS_DIR)/qt-5.2.1 -MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32 -PYTHON_DIR := $(QT_SDK_DIR)/Tools/mingw48_32/opt/bin -NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode -SDL_DIR := $(TOOLS_DIR)/SDL-1.2.15 -OPENSSL_DIR := $(TOOLS_DIR)/openssl-1.0.1e-win32 +QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1 UNCRUSTIFY_DIR := $(TOOLS_DIR)/uncrustify-0.60 DOXYGEN_DIR := $(TOOLS_DIR)/doxygen-1.8.3.1 GTEST_DIR := $(TOOLS_DIR)/gtest-1.6.0 -MESAWIN_DIR := $(TOOLS_DIR)/mesawin + +ifeq ($(UNAME), Windows) + MINGW_DIR := $(QT_SDK_DIR)/Tools/$(QT_SDK_ARCH) + PYTHON_DIR := $(QT_SDK_DIR)/Tools/$(QT_SDK_ARCH)/opt/bin + NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode + SDL_DIR := $(TOOLS_DIR)/SDL-1.2.15 + OPENSSL_DIR := $(TOOLS_DIR)/openssl-1.0.1e-win32 + MESAWIN_DIR := $(TOOLS_DIR)/mesawin +endif QT_SDK_PREFIX := $(QT_SDK_DIR) @@ -353,14 +356,14 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) $(DL_DIR)/$(5) --dump-binary-data -o $(1) # Extract packages under tool directory $(V1) $(MKDIR) -p $$(call toprel, $(dir $(2))) - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1mingw48_essentials.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1x32-4.8.0-release-posix-dwarf-rev2-runtime.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1icu_51_1_mingw_builds_4_8_0_posix_dwarf_32.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.addons/5.2.1mingw48_addons.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw48/4.8.0-1-1x32-4.8.0-release-posix-dwarf-rev2.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0qt5_essentials.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0i686-4.8.2-release-posix-dwarf-rt_v3-rev3-runtime.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0icu_52_1_mingw_builds_32_4_8_2_posix_dwarf.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0qt5_addons.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw482/4.8.2i686-4.8.2-release-posix-dwarf-rt_v3-rev3.7z" | grep -v Extracting # Run patcher @$(ECHO) @$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX)) @@ -502,7 +505,7 @@ endef ifeq ($(UNAME), Windows) -QT_SDK_PREFIX := $(QT_SDK_DIR)/5.2.1/$(QT_SDK_ARCH) +QT_SDK_PREFIX := $(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH) # This additional configuration step should not be necessary # but it is needed as a workaround to https://bugreports.qt-project.org/browse/QTBUG-33254 From c60417badb6a83ff28ab1cf8062c19b47cf20c2f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 14 Jul 2014 18:21:14 +0200 Subject: [PATCH 061/718] OP-1398 OP-1399 Windows Qt 5.3.1 upgrade - updated ICU version from 51 to 52 in copydata.pro --- ground/openpilotgcs/copydata.pro | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 933b77d76..f31e92504 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -142,9 +142,9 @@ GCS_LIBRARY_PATH Qt5MultimediaWidgets$${DS}.dll \ Qt5Quick$${DS}.dll \ Qt5Qml$${DS}.dll \ - icuin51.dll \ - icudt51.dll \ - icuuc51.dll + icuin52.dll \ + icudt52.dll \ + icuuc52.dll # it is more robust to take the following DLLs from Qt rather than from MinGW QT_DLLS += libgcc_s_dw2-1.dll \ libstdc++-6.dll \ From e196a1f01b0e485f71d0246712c09833c3d9c25f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 14 Jul 2014 18:24:42 +0200 Subject: [PATCH 062/718] OP-1398 OP-1399 Windows Qt 5.3.1 upgrade - removed duplicate method declarations in ophid header (were added in standard hidsdi.h header?) --- ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h index a2f8826b7..eda27387f 100644 --- a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h +++ b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h @@ -67,11 +67,7 @@ HIDAPI BOOL WINAPI HidD_FreePreparsedData(PHIDP_PREPARSED_DATA); HIDAPI BOOL WINAPI HidD_FlushQueue(HANDLE); HIDAPI BOOL WINAPI HidD_GetConfiguration(HANDLE, PHIDD_CONFIGURATION, ULONG); HIDAPI BOOL WINAPI HidD_SetConfiguration(HANDLE, PHIDD_CONFIGURATION, ULONG); -HIDAPI BOOL WINAPI HidD_GetNumInputBuffers(HANDLE, PULONG); -HIDAPI BOOL WINAPI HidD_SetNumInputBuffers(HANDLE HidDeviceObject, ULONG); HIDAPI BOOL WINAPI HidD_GetPhysicalDescriptor(HANDLE, PVOID, ULONG); -HIDAPI BOOL WINAPI HidD_GetManufacturerString(HANDLE, PVOID, ULONG); -HIDAPI BOOL WINAPI HidD_GetProductString(HANDLE, PVOID, ULONG); HIDAPI BOOL WINAPI HidD_GetIndexedString(HANDLE, ULONG, PVOID, ULONG); HIDAPI BOOL WINAPI HidD_GetSerialNumberString(HANDLE, PVOID, ULONG); From 89bc0b13f2a5363fc08fff750ffb53a929ef54f7 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 14 Jul 2014 20:03:19 +0200 Subject: [PATCH 063/718] OP-1398 OP-1399 Windows Qt 5.3.1 upgrade - workaround for QSerialPortInfo::availablePorts() returning spurious ports (QTBUG-39748) --- .../plugins/serialconnection/serialplugin.cpp | 21 +++++++++++++++++-- .../plugins/serialconnection/serialplugin.h | 2 ++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp index 59d130c02..408e46773 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp @@ -114,12 +114,29 @@ bool sortPorts(const QSerialPortInfo &s1, const QSerialPortInfo &s2) return s1.portName() < s2.portName(); } +QList SerialConnection::availablePorts() +{ + QList ports = QSerialPortInfo::availablePorts(); +#if QT_VERSION == 0x050301 && defined(Q_OS_WIN) + // workaround to QTBUG-39748 (https://bugreports.qt-project.org/browse/QTBUG-39748) + // Qt 5.3.1 reports spurious ports with an empty description... + QMutableListIterator i(ports); + while (i.hasNext()) { + if (i.next().description().isEmpty()) { + i.remove(); + } + } +#endif + return ports; +} + + QList SerialConnection::availableDevices() { QList list; if (enablePolling) { - QList ports = QSerialPortInfo::availablePorts(); + QList ports = availablePorts(); // sort the list by port number (nice idea from PT_Dreamer :)) qSort(ports.begin(), ports.end(), sortPorts); @@ -140,7 +157,7 @@ QIODevice *SerialConnection::openDevice(const QString &deviceName) if (serialHandle) { closeDevice(deviceName); } - QList ports = QSerialPortInfo::availablePorts(); + QList ports = availablePorts(); foreach(QSerialPortInfo port, ports) { if (port.portName() == deviceName) { // we need to handle port settings here... diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h index 14031a871..75385cea4 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h @@ -106,6 +106,8 @@ private: SerialPluginConfiguration *m_config; SerialPluginOptionsPage *m_optionspage; + QList availablePorts(); + protected slots: void onEnumerationChanged(); From ddde6d1f5e7f059047d27b8f178d87b3d550a9de Mon Sep 17 00:00:00 2001 From: Andres <> Date: Sun, 20 Jul 2014 15:26:05 +0200 Subject: [PATCH 064/718] OP-1317 changed int into int32_t in function Quaternion2PY() --- flight/modules/Airspeed/imu_airspeed.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 9c5ffd789..074f803e8 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -97,10 +97,10 @@ static void Quaternion2PY(const float q0, const float q1, const float q2, const } else { // calculate needed mutliples of 2pi to avoid jumps // number of cycles accumulated in old yaw - const int cycles = (int)(*yPtr / TWO_PI); + const int32_t cycles = (int32_t)(*yPtr / TWO_PI); // look for a jump by substracting the modulus, i.e. there is maximally one jump. // take slightly less than 2pi, because the jump will always be lower than 2pi - const int mod = (int)((y_ - (*yPtr - cycles * TWO_PI)) / (TWO_PI * 0.9f)); + const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * TWO_PI)) / (TWO_PI * 0.8f)); *yPtr = y_ + TWO_PI * (cycles - mod); } } @@ -222,7 +222,7 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air PY2xB(p, y, dxB); // calculate change in fuselage vector by substraction of old value PY2DeltaxB(imu->pOld, imu->yOld, dxB); - + // filter ground speed from VelocityState const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); From df4d211f8bd71734ca4029c1d24ad2a3f5f17b36 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Sun, 20 Jul 2014 16:22:27 +0200 Subject: [PATCH 065/718] OP-1317 added a proper initialization of the Butterworth filter values --- flight/modules/Airspeed/airspeed.c | 2 +- flight/modules/Airspeed/imu_airspeed.c | 55 +++++++++++++++++----- flight/modules/Airspeed/inc/imu_airspeed.h | 4 +- 3 files changed, 46 insertions(+), 15 deletions(-) diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index b01f14e7a..abafad6c5 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -165,7 +165,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters) case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION: if (!imuAirspeedInitialized) { imuAirspeedInitialized = true; - imu_airspeedInitialize(); + imu_airspeedInitialize(&airspeedSettings); } break; } diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 90adeaaef..8d0f09733 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -145,12 +145,40 @@ float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float return val; } +// Initialization function for direct form 2 Butterworth filter +void InitButterWorthDF2(const float ff, const float x0, float *wn1Ptr, float *wn2Ptr) +{ + const float ita = 1.0f / tanf(M_PI_F * ff); + const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); + const float b1 = 2.0f * b0; + const float b2 = b0; + const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); + const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); + + const float a11 = b1 + b0 * a1; + const float a12 = b2 + b0 * a2; + const float a21 = b1 + b0 * (Sq(a1) + a2); + const float a22 = b2 + b0 * a1 * a2; + const float det = a11 * a22 - a12 * a21; + + const float rhs1 = x0 - b0 * x0; + const float rhs2 = x0 - b0 * (x0 + a1 * x0); + + *wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; + *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; +} + /* * Initialize function loads first data sets, and allocates memory for structure. */ -void imu_airspeedInitialize() +void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings) { + // pre-filter frequency rate + const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1; + // filter frequency rate + const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2; + // This method saves memory in case we don't use the module. imu = (struct IMUGlobals *)pvPortMalloc(sizeof(struct IMUGlobals)); @@ -164,19 +192,22 @@ void imu_airspeedInitialize() // get pitch and yaw from quarternion; principal argument for yaw Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true); + InitButterWorthDF2(ff, imu->pOld, &(imu->pn1), &(imu->pn2)); + InitButterWorthDF2(ff, imu->yOld, &(imu->yn1), &(imu->yn2)); - imu->pn1 = imu->pn2 = imu->pOld; - imu->yn1 = imu->yn2 = imu->yOld; - - imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North; - imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East; - imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down; + // use current NED speed as vOld vector and as initial value for filter + imu->v1Old = velData.North; + imu->v2Old = velData.East; + imu->v3Old = velData.Down; + InitButterWorthDF2(ff, imu->v1Old, &(imu->v1n1), &(imu->v1n2)); + InitButterWorthDF2(ff, imu->v2Old, &(imu->v2n1), &(imu->v2n2)); + InitButterWorthDF2(ff, imu->v3Old, &(imu->v3n1), &(imu->v3n2)); // initial guess for windspeed is zero - imu->Vw1n1 = imu->Vw1n2 = 0.0f; - imu->Vw2n1 = imu->Vw2n2 = 0.0f; - imu->Vw3n1 = imu->Vw3n2 = 0.0f; - imu->Vw1 = imu->Vw2 = 0.0f; + imu->Vw3 = imu->Vw2 = imu->Vw1 = 0.0f; + InitButterWorthDF2(ffV, 0.0f, &(imu->Vw1n1), &(imu->Vw1n2)); + imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1; + imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2; } /* @@ -196,7 +227,7 @@ void imu_airspeedInitialize() * and yaw to keep a unit length. After building the differenced dxB and dVel are produced and * the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter */ -void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings) +void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings) { // pre-filter frequency rate const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1; diff --git a/flight/modules/Airspeed/inc/imu_airspeed.h b/flight/modules/Airspeed/inc/imu_airspeed.h index 88fe73b79..47544d74c 100644 --- a/flight/modules/Airspeed/inc/imu_airspeed.h +++ b/flight/modules/Airspeed/inc/imu_airspeed.h @@ -31,8 +31,8 @@ #ifndef IMU_AIRSPEED_H #define IMU_AIRSPEED_H -void imu_airspeedInitialize(); -void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings); +void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings); +void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings); #endif // IMU_AIRSPEED_H From ffabbc577c73964d6c2c540aaa7efcab24d4f16c Mon Sep 17 00:00:00 2001 From: Andres <> Date: Tue, 22 Jul 2014 20:03:43 +0200 Subject: [PATCH 066/718] OP-1317 moved Butterworth filter code into flight/libraries/math/butterworth.* --- flight/modules/Airspeed/imu_airspeed.c | 136 ++++++++++++++---- .../targets/boards/simposix/firmware/Makefile | 1 + make/apps-defs.mk | 1 + 3 files changed, 112 insertions(+), 26 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index 8d0f09733..da43c4de9 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -37,18 +37,25 @@ #include "airspeedsettings.h" #include "imu_airspeed.h" #include "CoordinateConversions.h" +#include "butterworth.h" #include // Private constants #define TWO_PI 6.283185308f -#define SQRT2 1.414213562f +#define EPS 1e-6f #define EPS_REORIENTATION 1e-10f #define EPS_VELOCITY 1.f // Private types -// structure with smoothed fuselage orientation, ground speed and their changes in time +// structure with smoothed fuselage orientation, ground speed, wind vector and their changes in time struct IMUGlobals { + // Butterworth filters + struct ButterWorthDF2Filter filter; + struct ButterWorthDF2Filter prefilter; + float ff, ffV; + + // storage variables for Butterworth filter float pn1, pn2; float yn1, yn2; @@ -130,8 +137,8 @@ static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[ // second order Butterworth filter with cut-off frequency ratio ff // Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored // function takes care of updating the values wn1 and wn2 -float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float *wn2Ptr) -{ +/*float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float *wn2Ptr) + { const float ita = 1.0f / tanf(M_PI_F * ff); const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); @@ -140,14 +147,30 @@ float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float const float wn = xn + a1 * (*wn1Ptr) + a2 * (*wn2Ptr); const float val = b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); - *wn2Ptr = *wn1Ptr; - *wn1Ptr = wn; + * wn2Ptr = *wn1Ptr; + * wn1Ptr = wn; return val; -} + }*/ + + +// second order Butterworth filter with cut-off frequency ratio ff +// Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored +// function takes care of updating the values wn1 and wn2 +// float FilterButterWorthDF2(const float xn, const ButterWorthDF2Filter *filter, float *wn1Ptr, float *wn2Ptr) +// { +// +// const float wn = xn + filter->a1 * (*wn1Ptr) + filter->a2 * (*wn2Ptr); +// const float val = filter->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); +// +// *wn2Ptr = *wn1Ptr; +// *wn1Ptr = wn; +// return val; +// } + // Initialization function for direct form 2 Butterworth filter -void InitButterWorthDF2(const float ff, const float x0, float *wn1Ptr, float *wn2Ptr) -{ +/*void InitButterWorthDF2(const float ff, const float x0, float *wn1Ptr, float *wn2Ptr) + { const float ita = 1.0f / tanf(M_PI_F * ff); const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); const float b1 = 2.0f * b0; @@ -164,9 +187,48 @@ void InitButterWorthDF2(const float ff, const float x0, float *wn1Ptr, float *wn const float rhs1 = x0 - b0 * x0; const float rhs2 = x0 - b0 * (x0 + a1 * x0); - *wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; - *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; -} + * wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; + * wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; + }*/ + + +// Initialization function for direct form 2 Butterworth filter +// initialize filter constants. Note that the following is used: +// b1 = 2 * b0 +// b2 = b0 +// void InitButterWorthDF2Filter(const float ff, ButterWorthDF2Filter *filter) +// { +// const float ita = 1.0f / tanf(M_PI_F * ff); +// const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); +// const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); +// const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); +// +// filter->ff = ff; +// filter->b0 = b0; +// filter->a1 = a1; +// filter->a2 = a2; +// } + + +//// Initialization function for direct form 2 Butterworth filter +//// initialize intermediate values for starting value x0 +// void InitButterWorthDF2Values(const float x0, const ButterWorthDF2Filter *filter, float *wn1Ptr, float *wn2Ptr) +// { +// const float b0 = filter->b0; +// const float a1 = filter->a1; +// const float a2 = filter->a2; +// +// const float a11 = 2.0f + a1; +// const float a12 = 1.0f + a2; +// const float a21 = 2.0f + Sq(a1) + a2; +// const float a22 = 1.0f + a1 * a2; +// const float det = a11 * a22 - a12 * a21; +// const float rhs1 = x0 / b0 - x0; +// const float rhs2 = x0 / b0 - x0 + a1 * x0; +// +// *wn1Ptr = ( a22 * rhs1 - a12 * rhs2) / det; +// *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; +// } /* @@ -190,22 +252,28 @@ void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings) AttitudeStateData attData; AttitudeStateGet(&attData); + // initialize filters for given ff and ffV + InitButterWorthDF2Filter(ffV, &(imu->filter)); + InitButterWorthDF2Filter(ff, &(imu->prefilter)); + imu->ffV = ffV; + imu->ff = ff; + // get pitch and yaw from quarternion; principal argument for yaw Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true); - InitButterWorthDF2(ff, imu->pOld, &(imu->pn1), &(imu->pn2)); - InitButterWorthDF2(ff, imu->yOld, &(imu->yn1), &(imu->yn2)); + InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); // use current NED speed as vOld vector and as initial value for filter imu->v1Old = velData.North; imu->v2Old = velData.East; imu->v3Old = velData.Down; - InitButterWorthDF2(ff, imu->v1Old, &(imu->v1n1), &(imu->v1n2)); - InitButterWorthDF2(ff, imu->v2Old, &(imu->v2n1), &(imu->v2n2)); - InitButterWorthDF2(ff, imu->v3Old, &(imu->v3n1), &(imu->v3n2)); + InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); // initial guess for windspeed is zero imu->Vw3 = imu->Vw2 = imu->Vw1 = 0.0f; - InitButterWorthDF2(ffV, 0.0f, &(imu->Vw1n1), &(imu->Vw1n2)); + InitButterWorthDF2Values(0.0f, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1; imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2; } @@ -234,6 +302,22 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsDat // filter frequency rate const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2; + // check for a change in filter frequency rate. if yes, then actualize filter constants and intermediate values + if (fabsf(ffV - imu->ffV) > EPS) { + InitButterWorthDF2Filter(ffV, &(imu->filter)); + InitButterWorthDF2Values(imu->Vw1, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); + InitButterWorthDF2Values(imu->Vw2, &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2)); + InitButterWorthDF2Values(imu->Vw3, &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2)); + } + if (fabsf(ff - imu->ff) > EPS) { + InitButterWorthDF2Filter(ff, &(imu->prefilter)); + InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); + InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); + } + float normVel2; float normDiffAttitude2; float dvdtDotdfdt; @@ -253,17 +337,17 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsDat Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false); // filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times - p = FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2)); - y = FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2)); + p = FilterButterWorthDF2(p, &(imu->prefilter), &(imu->pn1), &(imu->pn2)); + y = FilterButterWorthDF2(y, &(imu->prefilter), &(imu->yn1), &(imu->yn2)); // transform pitch and yaw into fuselage vector xB and xBold PY2xB(p, y, xB); // calculate change in fuselage vector by substraction of old value PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB); // filter ground speed from VelocityState - const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2)); - const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2)); - const float fv3n = FilterButterWorthDF2(ff, velData.Down, &(imu->v3n1), &(imu->v3n2)); + const float fv1n = FilterButterWorthDF2(velData.North, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2)); + const float fv2n = FilterButterWorthDF2(velData.East, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2)); + const float fv3n = FilterButterWorthDF2(velData.Down, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2)); // calculate norm of ground speed normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n); @@ -293,9 +377,9 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsDat imu->v2Old - xB[1] * airspeed, imu->v3Old - xB[2] * airspeed }; // filter raw wind - imu->Vw1 = FilterButterWorthDF2(ffV, wind[0], &(imu->Vw1n1), &(imu->Vw1n2)); - imu->Vw2 = FilterButterWorthDF2(ffV, wind[1], &(imu->Vw2n1), &(imu->Vw2n2)); - imu->Vw3 = FilterButterWorthDF2(ffV, wind[2], &(imu->Vw3n1), &(imu->Vw3n2)); + imu->Vw1 = FilterButterWorthDF2(wind[0], &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2)); + imu->Vw2 = FilterButterWorthDF2(wind[1], &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2)); + imu->Vw3 = FilterButterWorthDF2(wind[2], &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2)); } // else leave wind estimation unchanged { // Scoping to save memory diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 9eaa00725..8d3314640 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -100,6 +100,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c +SRC += $(MATHLIB)/butterworth.c SRC += $(PIOSCORECOMMON)/pios_task_monitor.c SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c diff --git a/make/apps-defs.mk b/make/apps-defs.mk index c65ade104..cc73d2c6e 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -107,6 +107,7 @@ SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c +SRC += $(MATHLIB)/butterworth.c SRC += $(FLIGHTLIB)/printf-stdarg.c ## Modules From 9589167914c88050846e7882f5f59af0ad1a2d18 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Tue, 22 Jul 2014 20:05:09 +0200 Subject: [PATCH 067/718] OP-1317 added Butterworth filter code files flight/libraries/math/butterworth.* --- flight/libraries/math/butterworth.c | 102 ++++++++++++++++++++++++++++ flight/libraries/math/butterworth.h | 46 +++++++++++++ 2 files changed, 148 insertions(+) create mode 100644 flight/libraries/math/butterworth.c create mode 100644 flight/libraries/math/butterworth.h diff --git a/flight/libraries/math/butterworth.c b/flight/libraries/math/butterworth.c new file mode 100644 index 000000000..eca00473e --- /dev/null +++ b/flight/libraries/math/butterworth.c @@ -0,0 +1,102 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Butterworth low pass filter + * @{ + * + * @file butterworth.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Direct form two of a second order Butterworth low pass filter + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 "openpilot.h" +#include "math.h" +#include "butterworth.h" + +#define SQRT2 1.414213562f + +/** + * Initialization function for coefficients of a second order Butterworth biquadratic filter in direct from 2. + * Note that b1 = 2 * b0 and b2 = b0 is use here and in the sequel. + * @param[in] ff Cut-off frequency ratio + * @param[out] filterPtr Pointer to filter coefficients + * @returns Nothing + */ +void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr) +{ + const float ita = 1.0f / tanf(M_PI_F * ff); + const float b0 = 1.0f / (1.0f + SQRT2 * ita + ita * ita); + const float a1 = 2.0f * b0 * (ita * ita - 1.0f); + const float a2 = -b0 * (1.0f - SQRT2 * ita + ita * ita); + + filterPtr->b0 = b0; + filterPtr->a1 = a1; + filterPtr->a2 = a2; +} + + +/** + * Initialization function for intermediate values of a second order Butterworth biquadratic filter in direct from 2. + * Obtained by solving a linear equation system. + * @param[in] x0 Prescribed value + * @param[in] filterPtr Pointer to filter coefficients + * @param[out] wn1Ptr Pointer to first intermediate value + * @param[out] wn2Ptr Pointer to second intermediate value + * @returns Nothing + */ +void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr) +{ + const float b0 = filterPtr->b0; + const float a1 = filterPtr->a1; + const float a2 = filterPtr->a2; + + const float a11 = 2.0f + a1; + const float a12 = 1.0f + a2; + const float a21 = 2.0f + a1 * a1 + a2; + const float a22 = 1.0f + a1 * a2; + const float det = a11 * a22 - a12 * a21; + const float rhs1 = x0 / b0 - x0; + const float rhs2 = x0 / b0 - x0 + a1 * x0; + + *wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; + *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; +} + + +/** + * Second order Butterworth biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored. + * Function takes care of updating the values wn1 and wn2. + * @param[in] xn New raw value + * @param[in] filterPtr Pointer to filter coefficients + * @param[out] wn1Ptr Pointer to first intermediate value + * @param[out] wn2Ptr Pointer to second intermediate value + * @returns Filtered value + */ +float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr) +{ + const float wn = xn + filterPtr->a1 * (*wn1Ptr) + filterPtr->a2 * (*wn2Ptr); + const float val = filterPtr->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); + + *wn2Ptr = *wn1Ptr; + *wn1Ptr = wn; + return val; +} diff --git a/flight/libraries/math/butterworth.h b/flight/libraries/math/butterworth.h new file mode 100644 index 000000000..126cd5ca4 --- /dev/null +++ b/flight/libraries/math/butterworth.h @@ -0,0 +1,46 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Butterworth low pass filter + * @{ + * + * @file butterworth.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Direct form two of a second order Butterworth low pass filter + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 BUTTERWORTH_H +#define BUTTERWORTH_H + +// Coefficients of second order Butterworth biquadratic filter in direct from 2 +struct ButterWorthDF2Filter { + float b0; + float a1; + float a2; +}; + +// Function declarations +void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr); +void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr); +float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr); + +#endif From d6a93cccaf414dd01c3cbb625d7cfebe7f00a365 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 23 Jul 2014 08:03:12 +0200 Subject: [PATCH 068/718] OP-1317 added M_2PI_F and M_2PI_D constants for 2*pi in pios_math.h --- flight/pios/inc/pios_math.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h index dc843999d..781e852c5 100644 --- a/flight/pios/inc/pios_math.h +++ b/flight/pios/inc/pios_math.h @@ -40,6 +40,7 @@ #define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */ #define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */ #define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */ +#define M_2PI_F 6.28318530717958647692528676656f /* 2pi */ #define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */ #define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */ #define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */ @@ -54,6 +55,7 @@ #define M_PI_D 3.14159265358979323846264338328d /* pi */ #define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */ #define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */ +#define M_2PI_D 6.28318530717958647692528676656d /* 2pi */ #define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */ #define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */ #define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */ From e195befc3aa69a6c08248bf0a856b41c016c4c64 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 23 Jul 2014 08:03:48 +0200 Subject: [PATCH 069/718] OP-1317 removed comments --- flight/modules/Airspeed/imu_airspeed.c | 104 +------------------------ 1 file changed, 3 insertions(+), 101 deletions(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index da43c4de9..bdceccc3a 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -42,7 +42,6 @@ // Private constants -#define TWO_PI 6.283185308f #define EPS 1e-6f #define EPS_REORIENTATION 1e-10f #define EPS_VELOCITY 1.f @@ -55,7 +54,6 @@ struct IMUGlobals { struct ButterWorthDF2Filter prefilter; float ff, ffV; - // storage variables for Butterworth filter float pn1, pn2; float yn1, yn2; @@ -106,11 +104,11 @@ static void Quaternion2PY(const float q0, const float q1, const float q2, const } else { // calculate needed mutliples of 2pi to avoid jumps // number of cycles accumulated in old yaw - const int32_t cycles = (int32_t)(*yPtr / TWO_PI); + const int32_t cycles = (int32_t)(*yPtr / M_2PI_F); // look for a jump by substracting the modulus, i.e. there is maximally one jump. // take slightly less than 2pi, because the jump will always be lower than 2pi - const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * TWO_PI)) / (TWO_PI * 0.8f)); - *yPtr = y_ + TWO_PI * (cycles - mod); + const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * M_2PI_F)) / (M_2PI_F * 0.8f)); + *yPtr = y_ + M_2PI_F * (cycles - mod); } } @@ -134,102 +132,6 @@ static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[ } -// second order Butterworth filter with cut-off frequency ratio ff -// Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored -// function takes care of updating the values wn1 and wn2 -/*float FilterButterWorthDF2(const float ff, const float xn, float *wn1Ptr, float *wn2Ptr) - { - const float ita = 1.0f / tanf(M_PI_F * ff); - const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); - const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); - const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); - - const float wn = xn + a1 * (*wn1Ptr) + a2 * (*wn2Ptr); - const float val = b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); - - * wn2Ptr = *wn1Ptr; - * wn1Ptr = wn; - return val; - }*/ - - -// second order Butterworth filter with cut-off frequency ratio ff -// Biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored -// function takes care of updating the values wn1 and wn2 -// float FilterButterWorthDF2(const float xn, const ButterWorthDF2Filter *filter, float *wn1Ptr, float *wn2Ptr) -// { -// -// const float wn = xn + filter->a1 * (*wn1Ptr) + filter->a2 * (*wn2Ptr); -// const float val = filter->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr)); -// -// *wn2Ptr = *wn1Ptr; -// *wn1Ptr = wn; -// return val; -// } - - -// Initialization function for direct form 2 Butterworth filter -/*void InitButterWorthDF2(const float ff, const float x0, float *wn1Ptr, float *wn2Ptr) - { - const float ita = 1.0f / tanf(M_PI_F * ff); - const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); - const float b1 = 2.0f * b0; - const float b2 = b0; - const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); - const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); - - const float a11 = b1 + b0 * a1; - const float a12 = b2 + b0 * a2; - const float a21 = b1 + b0 * (Sq(a1) + a2); - const float a22 = b2 + b0 * a1 * a2; - const float det = a11 * a22 - a12 * a21; - - const float rhs1 = x0 - b0 * x0; - const float rhs2 = x0 - b0 * (x0 + a1 * x0); - - * wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det; - * wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; - }*/ - - -// Initialization function for direct form 2 Butterworth filter -// initialize filter constants. Note that the following is used: -// b1 = 2 * b0 -// b2 = b0 -// void InitButterWorthDF2Filter(const float ff, ButterWorthDF2Filter *filter) -// { -// const float ita = 1.0f / tanf(M_PI_F * ff); -// const float b0 = 1.0f / (1.0f + SQRT2 * ita + Sq(ita)); -// const float a1 = 2.0f * b0 * (Sq(ita) - 1.0f); -// const float a2 = -b0 * (1.0f - SQRT2 * ita + Sq(ita)); -// -// filter->ff = ff; -// filter->b0 = b0; -// filter->a1 = a1; -// filter->a2 = a2; -// } - - -//// Initialization function for direct form 2 Butterworth filter -//// initialize intermediate values for starting value x0 -// void InitButterWorthDF2Values(const float x0, const ButterWorthDF2Filter *filter, float *wn1Ptr, float *wn2Ptr) -// { -// const float b0 = filter->b0; -// const float a1 = filter->a1; -// const float a2 = filter->a2; -// -// const float a11 = 2.0f + a1; -// const float a12 = 1.0f + a2; -// const float a21 = 2.0f + Sq(a1) + a2; -// const float a22 = 1.0f + a1 * a2; -// const float det = a11 * a22 - a12 * a21; -// const float rhs1 = x0 / b0 - x0; -// const float rhs2 = x0 / b0 - x0 + a1 * x0; -// -// *wn1Ptr = ( a22 * rhs1 - a12 * rhs2) / det; -// *wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det; -// } - /* * Initialize function loads first data sets, and allocates memory for structure. From 7f8e709e175802dab37e72d4036a7c21aba62aa3 Mon Sep 17 00:00:00 2001 From: Andres <> Date: Wed, 23 Jul 2014 08:04:52 +0200 Subject: [PATCH 070/718] OP-1317 uncrustify --- flight/modules/Airspeed/imu_airspeed.c | 1 - 1 file changed, 1 deletion(-) diff --git a/flight/modules/Airspeed/imu_airspeed.c b/flight/modules/Airspeed/imu_airspeed.c index bdceccc3a..d9c9414cf 100644 --- a/flight/modules/Airspeed/imu_airspeed.c +++ b/flight/modules/Airspeed/imu_airspeed.c @@ -132,7 +132,6 @@ static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[ } - /* * Initialize function loads first data sets, and allocates memory for structure. */ From 3e2961420d777eee37042576e7e953248430df89 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 15 Jul 2014 09:30:24 +0200 Subject: [PATCH 071/718] Add a 3D mode to path_endpoint. This allows FlyDirect waypoints to do vertical movements without changing position. --- flight/libraries/inc/paths.h | 4 +- flight/libraries/paths.c | 43 ++++++++++++------- .../VtolPathFollower/vtolpathfollower.c | 19 ++++---- 3 files changed, 40 insertions(+), 26 deletions(-) diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index bbb3995fe..4cecbe845 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -30,8 +30,8 @@ struct path_status { float fractional_progress; float error; - float correction_direction[2]; - float path_direction[2]; + float correction_direction[3]; + float path_direction[3]; }; void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 1cbddb7d2..831f3e98f 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -34,7 +34,7 @@ // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); @@ -65,10 +65,13 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc break; case PATHDESIRED_MODE_FLYENDPOINT: + return path_endpoint(start_point, end_point, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEENDPOINT: default: // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status); + return path_endpoint(start_point, end_point, cur_point, status, false); break; } @@ -80,30 +83,33 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc * @param[in] end_point Ending point * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float path[3], diff[3]; float dist_path, dist_diff; // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = 0; + status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to end - diff_north = end_point[0] - cur_point[0]; - diff_east = end_point[1] - cur_point[1]; + diff[0] = end_point[0] - cur_point[0]; + diff[1] = end_point[1] - cur_point[1]; + diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; - dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east); - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]); + dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]); if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[0] = status->path_direction[1] = status->path_direction[2] = 0; return; } @@ -111,8 +117,9 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point status->error = dist_diff; // Compute direction to travel - status->path_direction[0] = diff_north / dist_diff; - status->path_direction[1] = diff_east / dist_diff; + status->path_direction[0] = diff[0] / dist_diff; + status->path_direction[1] = diff[1] / dist_diff; + status->path_direction[2] = diff[2] / dist_diff; } /** @@ -144,7 +151,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // if the path is too short, we cannot determine vector direction. // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status); + path_endpoint(start_point, end_point, cur_point, status, false); status->fractional_progress = 1; return; } @@ -159,6 +166,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // Compute direction to correct error status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; + status->correction_direction[2] = 0; // Now just want magnitude of error status->error = fabs(status->error); @@ -166,6 +174,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, // Compute direction to travel status->path_direction[0] = path_north / dist_path; status->path_direction[1] = path_east / dist_path; + status->path_direction[2] = 0; } /** @@ -200,8 +209,10 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, status->error = radius; status->correction_direction[0] = 0; status->correction_direction[1] = 1; + status->correction_direction[2] = 0; status->path_direction[0] = 1; status->path_direction[1] = 0; + status->path_direction[2] = 0; return; } @@ -246,10 +257,12 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, // Compute direction to correct error status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; + status->correction_direction[2] = 0; // Compute direction to travel status->path_direction[0] = normal[0]; status->path_direction[1] = normal[1]; + status->path_direction[2] = 0; status->error = fabs(status->error); } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 57d96fad9..34b4bfaea 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -387,35 +387,36 @@ static void updatePathVelocity() cast_struct_to_array(pathDesired.End, pathDesired.End.North), cur, &progress, pathDesired.Mode); - float groundspeed; + float speed; switch (pathDesired.Mode) { case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; + speed = pathDesired.EndingVelocity; break; case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: - groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); + speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { - groundspeed = 0; + speed = 0; } break; case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR: default: - groundspeed = pathDesired.StartingVelocity + speed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { - groundspeed = 0; + speed = 0; } break; } VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * groundspeed; - velocityDesired.East = progress.path_direction[1] * groundspeed; + velocityDesired.North = progress.path_direction[0] * speed; + velocityDesired.East = progress.path_direction[1] * speed; + velocityDesired.Down = progress.path_direction[2] * speed; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; float correction_velocity[2] = @@ -436,7 +437,7 @@ static void updatePathVelocity() downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, -vtolpathfollowerSettings.VerticalPosPI.ILimit, vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); + downCommand = velocityDesired.Down + (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus From 7da0034775f5919cad2e5e92d267dee12322020c Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 15 Jul 2014 09:39:13 +0200 Subject: [PATCH 072/718] Improved automatic landing algorithm to better track descend speed. Desired descend speed is now a FlightModeSetting. --- flight/libraries/plans.c | 22 ++++++++++++++++--- .../flightmodesettings.xml | 1 + 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 9055419fa..10df7e52c 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -128,9 +128,19 @@ void plan_setup_returnToBase() PathDesiredSet(&pathDesired); } +static PiOSDeltatimeConfig landdT; void plan_setup_land() { + float descendspeed; plan_setup_positionHold(); + + FlightModeSettingsLandingVelocityGet(&descendspeed); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.StartingVelocity = descendspeed; + pathDesired.EndingVelocity = descendspeed; + PathDesiredSet(&pathDesired); + PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } /** @@ -138,12 +148,18 @@ void plan_setup_land() */ void plan_run_land() { + float downPos, descendspeed; PathDesiredEndData pathDesiredEnd; - PathDesiredEndGet(&pathDesiredEnd); + PositionStateDownGet(&downPos); // current down position + PathDesiredEndGet(&pathDesiredEnd); // desired position + PathDesiredEndingVelocityGet(&descendspeed); - PositionStateDownGet(&pathDesiredEnd.Down); - pathDesiredEnd.Down += 5; + // desired position is updated to match the desired descend speed but don't run ahead + // too far if the current position can't keep up. This normaly means we have landed. + if (pathDesiredEnd.Down - downPos < 10) { + pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT); + } PathDesiredEndSet(&pathDesiredEnd); } diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index b4b5a1c50..728fc27c0 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -110,6 +110,7 @@ + From 065ba1a0c81972be3b54914aa46648fb689dc3b2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 29 Jul 2014 13:06:40 +0200 Subject: [PATCH 073/718] Adds a 3D mode to path_vector. This allows vertical path segments without position changes. PathStatus now also contains correction_direction and path_direction to make path following behaviour more transparent. --- flight/libraries/paths.c | 89 ++++++++++++------- .../VtolPathFollower/vtolpathfollower.c | 62 +++++++------ shared/uavobjectdefinition/pathstatus.xml | 7 +- 3 files changed, 97 insertions(+), 61 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 831f3e98f..d8da67299 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -26,6 +26,7 @@ #include #include +#include #include "paths.h" @@ -35,7 +36,7 @@ // private functions static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status); +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); /** @@ -50,8 +51,11 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc { switch (mode) { case PATHDESIRED_MODE_FLYVECTOR: + return path_vector(start_point, end_point, cur_point, status, true); + + break; case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status); + return path_vector(start_point, end_point, cur_point, status, false); break; case PATHDESIRED_MODE_FLYCIRCLERIGHT: @@ -113,7 +117,11 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point return; } - status->fractional_progress = 1 - dist_diff / (1 + dist_path); + if (dist_path + 1 > dist_diff) { + status->fractional_progress = 1 - dist_diff / (1 + dist_path); + } else { + status->fractional_progress = 0; // we don't want fractional_progress to become negative + } status->error = dist_diff; // Compute direction to travel @@ -128,53 +136,70 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point * @param[in] end_point Ending point * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation + * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status) +static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) { - float path_north, path_east, diff_north, diff_east; + float path[3], diff[3]; float dist_path; float dot; - float normal[2]; + float track_point[3]; // Distance to go - path_north = end_point[0] - start_point[0]; - path_east = end_point[1] - start_point[1]; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to start - diff_north = cur_point[0] - start_point[0]; - diff_east = cur_point[1] - start_point[1]; + diff[0] = cur_point[0] - start_point[0]; + diff[1] = cur_point[1] - start_point[1]; + diff[2] = mode3D ? cur_point[2] - start_point[2]: 0; - dot = path_north * diff_north + path_east * diff_east; - dist_path = sqrtf(path_north * path_north + path_east * path_east); + dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; + dist_path = sqrtf(path[0] * path[0] + path[1] * path[1] + path[2] * path[2]); - if (dist_path < 1e-6f) { + if (dist_path > 1e-6f) { + // Compute direction to travel & progress + status->path_direction[0] = path[0] / dist_path; + status->path_direction[1] = path[1] / dist_path; + status->path_direction[2] = path[2] / dist_path; + status->fractional_progress = dot / (dist_path * dist_path); + } else { // if the path is too short, we cannot determine vector direction. - // Fly towards the endpoint to prevent flying away, - // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status, false); + // Assume progress=1 and zero-length path. + status->path_direction[0] = 0; + status->path_direction[1] = 0; + status->path_direction[2] = 0; status->fractional_progress = 1; - return; } - // Compute the normal to the path - normal[0] = -path_east / dist_path; - normal[1] = path_north / dist_path; + // Compute point on track that is closest to our current position. + // Limiting fractional_progress makes sure the resulting point is also + // limited to be between start and endpoint. + status->fractional_progress = boundf(status->fractional_progress, 0, 1); - status->fractional_progress = dot / (dist_path * dist_path); - status->error = normal[0] * diff_north + normal[1] * diff_east; + track_point[0] = status->fractional_progress * path[0] + start_point[0]; + track_point[1] = status->fractional_progress * path[1] + start_point[1]; + track_point[2] = status->fractional_progress * path[2] + start_point[2]; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0]; - status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1]; - status->correction_direction[2] = 0; + status->correction_direction[0] = track_point[0] - cur_point[0]; + status->correction_direction[1] = track_point[1] - cur_point[1]; + status->correction_direction[2] = track_point[2] - cur_point[2]; - // Now just want magnitude of error - status->error = fabs(status->error); + status->error = sqrt(status->correction_direction[0] * status->correction_direction[0] + + status->correction_direction[1] * status->correction_direction[1] + + status->correction_direction[2] * status->correction_direction[2]); - // Compute direction to travel - status->path_direction[0] = path_north / dist_path; - status->path_direction[1] = path_east / dist_path; - status->path_direction[2] = 0; + // Normalize correction_direction but avoid division by zero + if (status->error > 1e-6f) { + status->correction_direction[0] /= status->error; + status->correction_direction[1] /= status->error; + status->correction_direction[2] /= status->error; + } else { + status->correction_direction[0] = 0; + status->correction_direction[1] = 0; + status->correction_direction[2] = 1; + } } /** diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 34b4bfaea..7cb3f27db 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -370,22 +370,19 @@ static void updatePOIBearing() static void updatePathVelocity() { float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; - float downCommand; PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); PositionStateData positionState; PositionStateGet(&positionState); - float cur[3] = - { positionState.North, positionState.East, positionState.Down }; + float current_position[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; path_progress( cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + current_position, &progress, pathDesired.Mode); float speed; switch (pathDesired.Mode) { @@ -414,36 +411,45 @@ static void updatePathVelocity() } VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0] * speed; - velocityDesired.East = progress.path_direction[1] * speed; - velocityDesired.Down = progress.path_direction[2] * speed; - float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; - float correction_velocity[2] = - { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; + northPosIntegral += progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; + eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; + downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2)); - float scale = 1; - if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) { - scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel; + northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); + + velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + + progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + + progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + + progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; + + // Make sure the desired velocities don't exceed PathFollower limits. + float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); + + if (groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; + velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired; } - velocityDesired.North += progress.correction_direction[0] * error_speed * scale; - velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - - float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1); - - float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, - -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); - downCommand = velocityDesired.Down + (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); - velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_direction[0]; + pathStatus.path_direction_east = progress.path_direction[1]; + pathStatus.path_direction_down = progress.path_direction[2]; + pathStatus.correction_direction_north = progress.correction_direction[0]; + pathStatus.correction_direction_east = progress.correction_direction[1]; + pathStatus.correction_direction_down = progress.correction_direction[2]; VelocityDesiredSet(&velocityDesired); } diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 52352dc4a..812b7a1cf 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -7,7 +7,12 @@ - + + + + + + From 2113e1a2e1ab0cee6284638298861cc13522ffee Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 11 Jul 2014 23:11:45 +0200 Subject: [PATCH 074/718] OP-1397 Add initial support for attitude mode response (deg) --- flight/modules/TxPID/txpid.c | 30 ++++++++++++++++++++ shared/uavobjectdefinition/txpidsettings.xml | 1 + 2 files changed, 31 insertions(+) diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index a48a20967..85a65172e 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -83,6 +83,7 @@ // Private functions static void updatePIDs(UAVObjEvent *ev); static uint8_t update(float *var, float val); +static uint8_t updateResponsiveness(uint8_t *var, float val); static float scale(float val, float inMin, float inMax, float outMin, float outMax); /** @@ -235,6 +236,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: needsUpdateBank |= update(&bank.RollPI.ILimit, value); break; + case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP: + needsUpdateBank |= updateResponsiveness(&bank.RollMax, value); + break; case TXPIDSETTINGS_PIDS_PITCHRATEKP: needsUpdateBank |= update(&bank.PitchRatePID.Kp, value); break; @@ -256,6 +260,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: needsUpdateBank |= update(&bank.PitchPI.ILimit, value); break; + case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP: + needsUpdateBank |= updateResponsiveness(&bank.PitchMax, value); + break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: needsUpdateBank |= update(&bank.RollRatePID.Kp, value); needsUpdateBank |= update(&bank.PitchRatePID.Kp, value); @@ -284,6 +291,10 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.RollPI.ILimit, value); needsUpdateBank |= update(&bank.PitchPI.ILimit, value); break; + case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP: + needsUpdateBank |= updateResponsiveness(&bank.RollMax, value); + needsUpdateBank |= updateResponsiveness(&bank.PitchMax, value); + break; case TXPIDSETTINGS_PIDS_YAWRATEKP: needsUpdateBank |= update(&bank.YawRatePID.Kp, value); break; @@ -305,6 +316,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: needsUpdateBank |= update(&bank.YawPI.ILimit, value); break; + case TXPIDSETTINGS_PIDS_YAWATTITUDERESP: + needsUpdateBank |= updateResponsiveness(&bank.YawMax, value); + break; case TXPIDSETTINGS_PIDS_GYROTAU: needsUpdateStab |= update(&stab.GyroTau, value); break; @@ -389,6 +403,22 @@ static uint8_t update(float *var, float val) return 0; } +/** + * Updates var using val if needed. + * \returns 1 if updated, 0 otherwise + */ +static uint8_t updateResponsiveness(uint8_t *var, float val) +{ + // Just floor it for now. + uint8_t roundedVal = (uint8_t)val; + + if (*var != roundedVal) { + *var = roundedVal; + return 1; + } + return 0; +} + /** * @} */ diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 9f1a91579..174ca840c 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -18,6 +18,7 @@ Roll Attitude.Kp, Pitch Attitude.Kp, Roll+Pitch Attitude.Kp, Yaw Attitude.Kp, Roll Attitude.Ki, Pitch Attitude.Ki, Roll+Pitch Attitude.Ki, Yaw Attitude.Ki, Roll Attitude.ILimit, Pitch Attitude.ILimit, Roll+Pitch Attitude.ILimit, Yaw Attitude.ILimit, + Roll Attitude.Resp, Pitch Attitude.Resp, Roll+Pitch Attitude.Resp, Yaw Attitude.Resp, GyroTau" defaultvalue="Disabled"/> From a7ee6fa7642d92cb2cc4182cde9874d8ad9884da Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 11 Jul 2014 23:29:36 +0200 Subject: [PATCH 075/718] OP-1397 Add initial support for rate mode response (deg/s) --- flight/modules/TxPID/txpid.c | 13 +++++++++++++ shared/uavobjectdefinition/txpidsettings.xml | 1 + 2 files changed, 14 insertions(+) diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 85a65172e..c49ed49d7 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -227,6 +227,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: needsUpdateBank |= update(&bank.RollRatePID.ILimit, value); break; + case TXPIDSETTINGS_PIDS_ROLLRATERESP: + needsUpdateBank |= update(&bank.ManualRate.Roll, value); + break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: needsUpdateBank |= update(&bank.RollPI.Kp, value); break; @@ -251,6 +254,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); break; + case TXPIDSETTINGS_PIDS_PITCHRATERESP: + needsUpdateBank |= update(&bank.ManualRate.Pitch, value); + break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: needsUpdateBank |= update(&bank.PitchPI.Kp, value); break; @@ -279,6 +285,10 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.RollRatePID.ILimit, value); needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); break; + case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP: + needsUpdateBank |= update(&bank.ManualRate.Roll, value); + needsUpdateBank |= update(&bank.ManualRate.Pitch, value); + break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: needsUpdateBank |= update(&bank.RollPI.Kp, value); needsUpdateBank |= update(&bank.PitchPI.Kp, value); @@ -307,6 +317,9 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: needsUpdateBank |= update(&bank.YawRatePID.ILimit, value); break; + case TXPIDSETTINGS_PIDS_YAWRATERESP: + needsUpdateBank |= update(&bank.ManualRate.Yaw, value); + break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: needsUpdateBank |= update(&bank.YawPI.Kp, value); break; diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 174ca840c..e85774e9b 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -15,6 +15,7 @@ Roll Rate.Ki, Pitch Rate.Ki, Roll+Pitch Rate.Ki, Yaw Rate.Ki, Roll Rate.Kd, Pitch Rate.Kd, Roll+Pitch Rate.Kd, Yaw Rate.Kd, Roll Rate.ILimit, Pitch Rate.ILimit, Roll+Pitch Rate.ILimit, Yaw Rate.ILimit, + Roll Rate.Resp, Pitch Rate.Resp, Roll+Pitch Rate.Resp, Yaw Rate.Resp, Roll Attitude.Kp, Pitch Attitude.Kp, Roll+Pitch Attitude.Kp, Yaw Attitude.Kp, Roll Attitude.Ki, Pitch Attitude.Ki, Roll+Pitch Attitude.Ki, Yaw Attitude.Ki, Roll Attitude.ILimit, Pitch Attitude.ILimit, Roll+Pitch Attitude.ILimit, Yaw Attitude.ILimit, From 14770876cd747a63e5fbb08daadd68e855a36fce Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Tue, 15 Jul 2014 00:19:28 +0200 Subject: [PATCH 076/718] OP-1397 Change spinbox properties when a Responsiveness item is selected --- .../src/plugins/config/configtxpidwidget.cpp | 43 +++++++++++++++++++ .../src/plugins/config/configtxpidwidget.h | 1 + 2 files changed, 44 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 85657e0f3..4e58d2357 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -51,6 +51,10 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings())); connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings())); + connect(m_txpid->PID1, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID2, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID3, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true); addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1); @@ -88,6 +92,45 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget() // Do nothing } +void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selected_pid_type) +{ + QDoubleSpinBox *minPID; + QDoubleSpinBox *maxPID; + + qDebug() << "ConfigTxPIDWidget::updateSpinBoxProperties(" << selected_pid_type << ")"; + + QObject *obj = sender(); + if (obj == m_txpid->PID1) { + minPID = m_txpid->MinPID1; + maxPID = m_txpid->MaxPID1; + } else if (obj == m_txpid->PID2) { + minPID = m_txpid->MinPID2; + maxPID = m_txpid->MaxPID2; + } else if (obj == m_txpid->PID3) { + minPID = m_txpid->MinPID3; + maxPID = m_txpid->MaxPID3; + } else { + qDebug() << "updateSpinBoxProperties: Incorrect sender object"; + return; + } + + if (selected_pid_type.endsWith(".Resp")) { + minPID->setRange(0, 999); + maxPID->setRange(0, 999); + minPID->setSingleStep(1); + maxPID->setSingleStep(1); + minPID->setDecimals(0); + maxPID->setDecimals(0); + } else { + minPID->setRange(0, 99.99); + maxPID->setRange(0, 99.99); + minPID->setSingleStep(0.000100); + maxPID->setSingleStep(0.000100); + minPID->setDecimals(6); + maxPID->setDecimals(6); + } +} + void ConfigTxPIDWidget::refreshValues() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 38d2aa318..212bc2880 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -40,6 +40,7 @@ private: Ui_TxPIDWidget *m_txpid; private slots: + void updateSpinBoxProperties(const QString & selected_pid_type); void refreshValues(); void applySettings(); void saveSettings(); From 0416c39cdb505f7cf00b768824a51a0d84bfaac9 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Thu, 17 Jul 2014 12:21:12 +0200 Subject: [PATCH 077/718] OP-1397 Limit the Attitude Responsiveness values to 180 degrees --- .../src/plugins/config/configtxpidwidget.cpp | 26 +++++++++++++++---- .../src/plugins/config/configtxpidwidget.h | 2 +- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 4e58d2357..01930f17a 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -92,12 +92,22 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget() // Do nothing } -void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selected_pid_type) +static bool isResponsivenessType(const QString & pidType) +{ + return pidType.endsWith(".Resp"); +} + +static bool isAttitudeType(const QString & pidType) +{ + return pidType.contains("Attitude"); +} + +void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidType) { QDoubleSpinBox *minPID; QDoubleSpinBox *maxPID; - qDebug() << "ConfigTxPIDWidget::updateSpinBoxProperties(" << selected_pid_type << ")"; + qDebug() << "ConfigTxPIDWidget::updateSpinBoxProperties(" << selectedPidType << ")"; QObject *obj = sender(); if (obj == m_txpid->PID1) { @@ -114,9 +124,15 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selected_pid_typ return; } - if (selected_pid_type.endsWith(".Resp")) { - minPID->setRange(0, 999); - maxPID->setRange(0, 999); + if (isResponsivenessType(selectedPidType)) { + if (isAttitudeType(selectedPidType)) { + // Limit to 180 degrees. + minPID->setRange(0, 180); + maxPID->setRange(0, 180); + } else { + minPID->setRange(0, 999); + maxPID->setRange(0, 999); + } minPID->setSingleStep(1); maxPID->setSingleStep(1); minPID->setDecimals(0); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 212bc2880..ca4c61d55 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -40,7 +40,7 @@ private: Ui_TxPIDWidget *m_txpid; private slots: - void updateSpinBoxProperties(const QString & selected_pid_type); + void updateSpinBoxProperties(const QString & selectedPidType); void refreshValues(); void applySettings(); void saveSettings(); From 3e4737949ab0cd3f9df7f90a923bb1f8bbf8bb4e Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 21 Jul 2014 10:37:09 +0200 Subject: [PATCH 078/718] OP-1397 Make sure the Min/MaxPIDx widgit limits get set before the values are populated --- .../src/plugins/config/configtxpidwidget.cpp | 36 ++++++++++--------- .../src/plugins/config/configtxpidwidget.h | 2 +- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 01930f17a..d69f594a3 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -51,20 +51,21 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings())); connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings())); - connect(m_txpid->PID1, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); - connect(m_txpid->PID2, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); - connect(m_txpid->PID3, SIGNAL(activated(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID1, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID2, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID3, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true); - addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1); - addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2); - addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3); - addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1); addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2); addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3); + // It's important that the PIDx values are populated before the MinPIDx and MaxPIDx, + // otherwise the MinPIDx and MaxPIDx will be capped by the old spin box limits. The correct limits + // are set when updateSpinBoxProperties is called when the PIDx->currentTextChanged signal is sent. + // The binding order is reversed because the values are populated in reverse. + addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1); addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2); addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3); @@ -73,6 +74,10 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2); addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3); + addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1); + addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2); + addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3); + addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN); addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX); @@ -92,24 +97,23 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget() // Do nothing } -static bool isResponsivenessType(const QString & pidType) +static bool isResponsivenessOption(const QString & pidOption) { - return pidType.endsWith(".Resp"); + return pidOption.endsWith(".Resp"); } -static bool isAttitudeType(const QString & pidType) +static bool isAttitudeOption(const QString & pidOption) { - return pidType.contains("Attitude"); + return pidOption.contains("Attitude"); } -void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidType) +void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOption) { QDoubleSpinBox *minPID; QDoubleSpinBox *maxPID; - qDebug() << "ConfigTxPIDWidget::updateSpinBoxProperties(" << selectedPidType << ")"; - QObject *obj = sender(); + if (obj == m_txpid->PID1) { minPID = m_txpid->MinPID1; maxPID = m_txpid->MaxPID1; @@ -124,8 +128,8 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidType) return; } - if (isResponsivenessType(selectedPidType)) { - if (isAttitudeType(selectedPidType)) { + if (isResponsivenessOption(selectedPidOption)) { + if (isAttitudeOption(selectedPidOption)) { // Limit to 180 degrees. minPID->setRange(0, 180); maxPID->setRange(0, 180); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index ca4c61d55..67c700867 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -40,7 +40,7 @@ private: Ui_TxPIDWidget *m_txpid; private slots: - void updateSpinBoxProperties(const QString & selectedPidType); + void updateSpinBoxProperties(const QString & selectedPidOption); void refreshValues(); void applySettings(); void saveSettings(); From 12589abe7e4994a9c80bc6490ad127530bd8d1d2 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 21 Jul 2014 15:38:28 +0200 Subject: [PATCH 079/718] OP-1397 Read default values for selected PID option from StabilizationSettingsBankX --- .../src/plugins/config/configtxpidwidget.cpp | 128 +++++++++++++++++- .../src/plugins/config/configtxpidwidget.h | 1 + 2 files changed, 124 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index d69f594a3..fce1984a2 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -28,6 +28,9 @@ #include "configtxpidwidget.h" #include "txpidsettings.h" #include "hwsettings.h" +#include "stabilizationsettingsbank1.h" +#include "stabilizationsettingsbank2.h" +#include "stabilizationsettingsbank3.h" #include #include @@ -107,20 +110,127 @@ static bool isAttitudeOption(const QString & pidOption) return pidOption.contains("Attitude"); } +template +static float defaultValueForPidOption(const StabilizationSettingsBankX & bank, const QString & pidOption) +{ + if (pidOption == "Disabled") { + return 0.0f; + } else if (pidOption == "Roll Rate.Kp") { + return bank->getRollRatePID_Kp(); + } else if (pidOption == "Pitch Rate.Kp") { + return bank->getPitchRatePID_Kp(); + } else if (pidOption == "Roll+Pitch Rate.Kp") { + return bank->getRollRatePID_Kp(); + } else if (pidOption == "Yaw Rate.Kp") { + return bank->getYawRatePID_Kp(); + } else if (pidOption == "Roll Rate.Ki") { + return bank->getRollRatePID_Ki(); + } else if (pidOption == "Pitch Rate.Ki") { + return bank->getPitchRatePID_Ki(); + } else if (pidOption == "Roll+Pitch Rate.Ki") { + return bank->getRollRatePID_Ki(); + } else if (pidOption == "Yaw Rate.Ki") { + return bank->getYawRatePID_Ki(); + } else if (pidOption == "Roll Rate.Kd") { + return bank->getRollRatePID_Kd(); + } else if (pidOption == "Pitch Rate.Kd") { + return bank->getPitchRatePID_Kd(); + } else if (pidOption == "Roll+Pitch Rate.Kd") { + return bank->getRollRatePID_Kd(); + } else if (pidOption == "Yaw Rate.Kd") { + return bank->getYawRatePID_Kd(); + } else if (pidOption == "Roll Rate.ILimit") { + return bank->getRollRatePID_ILimit(); + } else if (pidOption == "Pitch Rate.ILimit") { + return bank->getPitchRatePID_ILimit(); + } else if (pidOption == "Roll+Pitch Rate.ILimit") { + return bank->getRollRatePID_ILimit(); + } else if (pidOption == "Yaw Rate.ILimit") { + return bank->getYawRatePID_ILimit(); + } else if (pidOption == "Roll Rate.Resp") { + return bank->getManualRate_Roll(); + } else if (pidOption == "Pitch Rate.Resp") { + return bank->getManualRate_Pitch(); + } else if (pidOption == "Roll+Pitch Rate.Resp") { + return bank->getManualRate_Roll(); + } else if (pidOption == "Yaw Rate.Resp") { + return bank->getManualRate_Yaw(); + } else if (pidOption == "Roll Attitude.Kp") { + return bank->getRollPI_Kp(); + } else if (pidOption == "Pitch Attitude.Kp") { + return bank->getPitchPI_Kp(); + } else if (pidOption == "Roll+Pitch Attitude.Kp") { + return bank->getRollPI_Kp(); + } else if (pidOption == "Yaw Attitude.Kp") { + return bank->getYawPI_Kp(); + } else if (pidOption == "Roll Attitude.Ki") { + return bank->getRollPI_Ki(); + } else if (pidOption == "Pitch Attitude.Ki") { + return bank->getPitchPI_Ki(); + } else if (pidOption == "Roll+Pitch Attitude.Ki") { + return bank->getRollPI_Ki(); + } else if (pidOption == "Yaw Attitude.Ki") { + return bank->getYawPI_Ki(); + } else if (pidOption == "Roll Attitude.ILimit") { + return bank->getRollPI_ILimit(); + } else if (pidOption == "Pitch Attitude.ILimit") { + return bank->getPitchPI_ILimit(); + } else if (pidOption == "Roll+Pitch Attitude.ILimit") { + return bank->getRollPI_ILimit(); + } else if (pidOption == "Yaw Attitude.ILimit") { + return bank->getYawPI_ILimit(); + } else if (pidOption == "Roll Attitude.Resp") { + return (float)bank->getRollMax(); + } else if (pidOption == "Pitch Attitude.Resp") { + return (float)bank->getPitchMax(); + } else if (pidOption == "Roll+Pitch Attitude.Resp") { + return (float)bank->getRollMax(); + } else if (pidOption == "Yaw Attitude.Resp") { + return bank->getYawMax(); + } + + qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption; + return 0.0f; +} + +float ConfigTxPIDWidget::getDefaultValueForPidOption(const QString & pidOption) +{ + if (pidOption == "GyroTau") { + // TODO: Implement + return 0.0f; + } + + uint bankNumber = m_txpid->pidBank->currentIndex() + 1; + + if (bankNumber == 1) { + StabilizationSettingsBank1 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank1"))); + return defaultValueForPidOption(bank, pidOption); + } else if (bankNumber == 2) { + StabilizationSettingsBank2 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank2"))); + return defaultValueForPidOption(bank, pidOption); + } else if (bankNumber == 3) { + StabilizationSettingsBank3 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank3"))); + return defaultValueForPidOption(bank, pidOption); + } else { + qDebug() << "getDefaultValueForPidOption: Incorrect bank number:" << bankNumber; + return 0.0f; + } +} + void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOption) { + QObject *PIDx = sender(); + QDoubleSpinBox *minPID; QDoubleSpinBox *maxPID; - QObject *obj = sender(); - - if (obj == m_txpid->PID1) { + if (PIDx == m_txpid->PID1) { minPID = m_txpid->MinPID1; maxPID = m_txpid->MaxPID1; - } else if (obj == m_txpid->PID2) { + } else if (PIDx == m_txpid->PID2) { minPID = m_txpid->MinPID2; maxPID = m_txpid->MaxPID2; - } else if (obj == m_txpid->PID3) { + } else if (PIDx == m_txpid->PID3) { minPID = m_txpid->MinPID3; maxPID = m_txpid->MaxPID3; } else { @@ -128,6 +238,9 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOptio return; } + // The ranges need to be setup before the values can be set, + // otherwise the value might be incorrectly capped. + if (isResponsivenessOption(selectedPidOption)) { if (isAttitudeOption(selectedPidOption)) { // Limit to 180 degrees. @@ -149,6 +262,11 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOptio minPID->setDecimals(6); maxPID->setDecimals(6); } + + float value = getDefaultValueForPidOption(selectedPidOption); + + minPID->setValue(value); + maxPID->setValue(value); } void ConfigTxPIDWidget::refreshValues() diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 67c700867..34a50c413 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -41,6 +41,7 @@ private: private slots: void updateSpinBoxProperties(const QString & selectedPidOption); + float getDefaultValueForPidOption(const QString & pidOption); void refreshValues(); void applySettings(); void saveSettings(); From 9ab6299247ef3f44b4c50a639f0535038a11a423 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:18:39 +0200 Subject: [PATCH 080/718] We don't want zero-length path_direction vector. --- flight/libraries/paths.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index d8da67299..5d438959a 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -113,7 +113,8 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = status->path_direction[2] = 0; + status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[2] = 1; return; } From 70e34f44cf24b3d7caa0637671933558ce0c6008 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:52:06 +0200 Subject: [PATCH 081/718] Revert to the original behaviour of calling "fly_endpoint" if we have a zero-length path. --- flight/libraries/paths.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 5d438959a..888fd43ab 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -166,12 +166,11 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { - // if the path is too short, we cannot determine vector direction. - // Assume progress=1 and zero-length path. - status->path_direction[0] = 0; - status->path_direction[1] = 0; - status->path_direction[2] = 0; + // Fly towards the endpoint to prevent flying away, + // but assume progress=1 either way. + path_endpoint(start_point, end_point, cur_point, status); status->fractional_progress = 1; + return; } // Compute point on track that is closest to our current position. From 446415457d059ba6c7a57d8a5332e7cb38dddf51 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 10:56:25 +0200 Subject: [PATCH 082/718] Don't limit the path to strictly be between start and endpoint. --- flight/libraries/paths.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 888fd43ab..8c6416406 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -174,10 +174,6 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } // Compute point on track that is closest to our current position. - // Limiting fractional_progress makes sure the resulting point is also - // limited to be between start and endpoint. - status->fractional_progress = boundf(status->fractional_progress, 0, 1); - track_point[0] = status->fractional_progress * path[0] + start_point[0]; track_point[1] = status->fractional_progress * path[1] + start_point[1]; track_point[2] = status->fractional_progress * path[2] + start_point[2]; From f29f7fa6803a270d1e5f55033c1ac0c35c77f4c2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 31 Jul 2014 16:31:16 +0200 Subject: [PATCH 083/718] Missing argument for path_endpoint added. --- flight/libraries/paths.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 8c6416406..19176c443 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -168,7 +168,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } else { // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status); + path_endpoint(start_point, end_point, cur_point, status, mode3D); status->fractional_progress = 1; return; } From f9343d9959a2f9fd45068a5c2752a58ce6815db3 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 1 Aug 2014 09:41:13 +0200 Subject: [PATCH 084/718] OP-1397 Fetch default values for GyroTau --- ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index fce1984a2..ad8db8cd9 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -28,6 +28,7 @@ #include "configtxpidwidget.h" #include "txpidsettings.h" #include "hwsettings.h" +#include "stabilizationsettings.h" #include "stabilizationsettingsbank1.h" #include "stabilizationsettingsbank2.h" #include "stabilizationsettingsbank3.h" @@ -196,8 +197,8 @@ static float defaultValueForPidOption(const StabilizationSettingsBankX & bank, c float ConfigTxPIDWidget::getDefaultValueForPidOption(const QString & pidOption) { if (pidOption == "GyroTau") { - // TODO: Implement - return 0.0f; + StabilizationSettings *stab = qobject_cast(getObject(QString("StabilizationSettings"))); + return stab->getGyroTau(); } uint bankNumber = m_txpid->pidBank->currentIndex() + 1; From 04d9d3191dab5ea562868c3779849738be4212b0 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 1 Aug 2014 10:50:52 +0200 Subject: [PATCH 085/718] OP-1397 Fix StabilizationSettingsBankX type for defaultValueForPidOption --- ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index ad8db8cd9..9672bac11 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -112,7 +112,7 @@ static bool isAttitudeOption(const QString & pidOption) } template -static float defaultValueForPidOption(const StabilizationSettingsBankX & bank, const QString & pidOption) +static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, const QString & pidOption) { if (pidOption == "Disabled") { return 0.0f; From b285df98c7495061954287e2c9c1b08a20c99902 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 1 Aug 2014 13:36:22 +0200 Subject: [PATCH 086/718] OP-1423 Populate TxPID Min/Max fields with the current values from the Stabilization page --- .../src/plugins/config/configtxpidwidget.cpp | 126 ++++++++++++++++++ .../src/plugins/config/configtxpidwidget.h | 2 + 2 files changed, 128 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 85657e0f3..5447f3ec1 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -28,6 +28,10 @@ #include "configtxpidwidget.h" #include "txpidsettings.h" #include "hwsettings.h" +#include "stabilizationsettings.h" +#include "stabilizationsettingsbank1.h" +#include "stabilizationsettingsbank2.h" +#include "stabilizationsettingsbank3.h" #include #include @@ -51,6 +55,10 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings())); connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings())); + connect(m_txpid->PID1, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID2, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID3, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true); addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1); @@ -88,6 +96,124 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget() // Do nothing } +template +static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, const QString & pidOption) +{ + if (pidOption == "Disabled") { + return 0.0f; + } else if (pidOption == "Roll Rate.Kp") { + return bank->getRollRatePID_Kp(); + } else if (pidOption == "Pitch Rate.Kp") { + return bank->getPitchRatePID_Kp(); + } else if (pidOption == "Roll+Pitch Rate.Kp") { + return bank->getRollRatePID_Kp(); + } else if (pidOption == "Yaw Rate.Kp") { + return bank->getYawRatePID_Kp(); + } else if (pidOption == "Roll Rate.Ki") { + return bank->getRollRatePID_Ki(); + } else if (pidOption == "Pitch Rate.Ki") { + return bank->getPitchRatePID_Ki(); + } else if (pidOption == "Roll+Pitch Rate.Ki") { + return bank->getRollRatePID_Ki(); + } else if (pidOption == "Yaw Rate.Ki") { + return bank->getYawRatePID_Ki(); + } else if (pidOption == "Roll Rate.Kd") { + return bank->getRollRatePID_Kd(); + } else if (pidOption == "Pitch Rate.Kd") { + return bank->getPitchRatePID_Kd(); + } else if (pidOption == "Roll+Pitch Rate.Kd") { + return bank->getRollRatePID_Kd(); + } else if (pidOption == "Yaw Rate.Kd") { + return bank->getYawRatePID_Kd(); + } else if (pidOption == "Roll Rate.ILimit") { + return bank->getRollRatePID_ILimit(); + } else if (pidOption == "Pitch Rate.ILimit") { + return bank->getPitchRatePID_ILimit(); + } else if (pidOption == "Roll+Pitch Rate.ILimit") { + return bank->getRollRatePID_ILimit(); + } else if (pidOption == "Yaw Rate.ILimit") { + return bank->getYawRatePID_ILimit(); + } else if (pidOption == "Roll Attitude.Kp") { + return bank->getRollPI_Kp(); + } else if (pidOption == "Pitch Attitude.Kp") { + return bank->getPitchPI_Kp(); + } else if (pidOption == "Roll+Pitch Attitude.Kp") { + return bank->getRollPI_Kp(); + } else if (pidOption == "Yaw Attitude.Kp") { + return bank->getYawPI_Kp(); + } else if (pidOption == "Roll Attitude.Ki") { + return bank->getRollPI_Ki(); + } else if (pidOption == "Pitch Attitude.Ki") { + return bank->getPitchPI_Ki(); + } else if (pidOption == "Roll+Pitch Attitude.Ki") { + return bank->getRollPI_Ki(); + } else if (pidOption == "Yaw Attitude.Ki") { + return bank->getYawPI_Ki(); + } else if (pidOption == "Roll Attitude.ILimit") { + return bank->getRollPI_ILimit(); + } else if (pidOption == "Pitch Attitude.ILimit") { + return bank->getPitchPI_ILimit(); + } else if (pidOption == "Roll+Pitch Attitude.ILimit") { + return bank->getRollPI_ILimit(); + } else if (pidOption == "Yaw Attitude.ILimit") { + return bank->getYawPI_ILimit(); + } + + qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption; + return 0.0f; +} + +float ConfigTxPIDWidget::getDefaultValueForPidOption(const QString & pidOption) +{ + if (pidOption == "GyroTau") { + StabilizationSettings *stab = qobject_cast(getObject(QString("StabilizationSettings"))); + return stab->getGyroTau(); + } + + uint bankNumber = m_txpid->pidBank->currentIndex() + 1; + + if (bankNumber == 1) { + StabilizationSettingsBank1 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank1"))); + return defaultValueForPidOption(bank, pidOption); + } else if (bankNumber == 2) { + StabilizationSettingsBank2 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank2"))); + return defaultValueForPidOption(bank, pidOption); + } else if (bankNumber == 3) { + StabilizationSettingsBank3 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank3"))); + return defaultValueForPidOption(bank, pidOption); + } else { + qDebug() << "getDefaultValueForPidOption: Incorrect bank number:" << bankNumber; + return 0.0f; + } +} + +void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOption) +{ + QObject *PIDx = sender(); + + QDoubleSpinBox *minPID; + QDoubleSpinBox *maxPID; + + if (PIDx == m_txpid->PID1) { + minPID = m_txpid->MinPID1; + maxPID = m_txpid->MaxPID1; + } else if (PIDx == m_txpid->PID2) { + minPID = m_txpid->MinPID2; + maxPID = m_txpid->MaxPID2; + } else if (PIDx == m_txpid->PID3) { + minPID = m_txpid->MinPID3; + maxPID = m_txpid->MaxPID3; + } else { + qDebug() << "updateSpinBoxProperties: Incorrect sender object"; + return; + } + + float value = getDefaultValueForPidOption(selectedPidOption); + + minPID->setValue(value); + maxPID->setValue(value); +} + void ConfigTxPIDWidget::refreshValues() { HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager()); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 38d2aa318..34a50c413 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -40,6 +40,8 @@ private: Ui_TxPIDWidget *m_txpid; private slots: + void updateSpinBoxProperties(const QString & selectedPidOption); + float getDefaultValueForPidOption(const QString & pidOption); void refreshValues(); void applySettings(); void saveSettings(); From 7f344da9778ae6b615e45dd846dadaca6f6b3690 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 1 Aug 2014 14:51:39 +0200 Subject: [PATCH 087/718] OP-1397 Round to instead of floor to uint8_t values in txpid.c --- flight/modules/TxPID/txpid.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index c49ed49d7..f356558e8 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -83,7 +83,7 @@ // Private functions static void updatePIDs(UAVObjEvent *ev); static uint8_t update(float *var, float val); -static uint8_t updateResponsiveness(uint8_t *var, float val); +static uint8_t updateUint8(uint8_t *var, float val); static float scale(float val, float inMin, float inMax, float outMin, float outMax); /** @@ -240,7 +240,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.RollPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP: - needsUpdateBank |= updateResponsiveness(&bank.RollMax, value); + needsUpdateBank |= updateUint8(&bank.RollMax, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKP: needsUpdateBank |= update(&bank.PitchRatePID.Kp, value); @@ -267,7 +267,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.PitchPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP: - needsUpdateBank |= updateResponsiveness(&bank.PitchMax, value); + needsUpdateBank |= updateUint8(&bank.PitchMax, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: needsUpdateBank |= update(&bank.RollRatePID.Kp, value); @@ -302,8 +302,8 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.PitchPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP: - needsUpdateBank |= updateResponsiveness(&bank.RollMax, value); - needsUpdateBank |= updateResponsiveness(&bank.PitchMax, value); + needsUpdateBank |= updateUint8(&bank.RollMax, value); + needsUpdateBank |= updateUint8(&bank.PitchMax, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKP: needsUpdateBank |= update(&bank.YawRatePID.Kp, value); @@ -330,7 +330,7 @@ static void updatePIDs(UAVObjEvent *ev) needsUpdateBank |= update(&bank.YawPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDERESP: - needsUpdateBank |= updateResponsiveness(&bank.YawMax, value); + needsUpdateBank |= updateUint8(&bank.YawMax, value); break; case TXPIDSETTINGS_PIDS_GYROTAU: needsUpdateStab |= update(&stab.GyroTau, value); @@ -420,10 +420,9 @@ static uint8_t update(float *var, float val) * Updates var using val if needed. * \returns 1 if updated, 0 otherwise */ -static uint8_t updateResponsiveness(uint8_t *var, float val) +static uint8_t updateUint8(uint8_t *var, float val) { - // Just floor it for now. - uint8_t roundedVal = (uint8_t)val; + uint8_t roundedVal = (uint8_t)roundf(val); if (*var != roundedVal) { *var = roundedVal; From 3c1b6892247fac59952c15c90f76c3022f598135 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 01:20:42 +0200 Subject: [PATCH 088/718] OP-1403 - Add generic I2C/SPI HMC5x83 mag driver --- flight/pios/common/pios_hmc5x83.c | 518 ++++++++++++++++++++++++++++++ flight/pios/inc/pios_hmc5x83.h | 132 ++++++++ flight/pios/pios.h | 6 + 3 files changed, 656 insertions(+) create mode 100644 flight/pios/common/pios_hmc5x83.c create mode 100644 flight/pios/inc/pios_hmc5x83.h diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c new file mode 100644 index 000000000..922b5fde7 --- /dev/null +++ b/flight/pios/common/pios_hmc5x83.c @@ -0,0 +1,518 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HMC5x83 HMC5x83 Functions + * @brief Deals with the hardware interface to the magnetometers + * @{ + * @file pios_hmc5x83.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief HMC5x83 Magnetic Sensor Functions from AHRS + * @see The GNU Public License (GPL) Version 3 + * + ****************************************************************************** + */ +/* + * 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 "pios.h" +#include + +#ifdef PIOS_INCLUDE_HMC5X83 + +/* Global Variables */ + +/* Local Types */ + +/* Local Variables */ +volatile bool pios_hmc5x83_data_ready; + +static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg); +// static int32_t PIOS_HMC5x83_Read(uint8_t address, uint8_t *buffer, uint8_t len); +// static int32_t PIOS_HMC5x83_Write(uint8_t address, uint8_t buffer); + +static const struct pios_hmc5x83_cfg *dev_cfg; +static uint32_t dev_port_id; +static uint8_t dev_slave_num; +/** + * @brief Initialize the HMC5x83 magnetometer sensor. + * @return none + */ +void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) +{ + dev_cfg = cfg; // store config before enabling interrupt + dev_port_id = port_id; + dev_slave_num = slave_num; +#ifdef PIOS_HMC5X83_HAS_GPIOS + PIOS_EXTI_Init(cfg->exti_cfg); +#endif + + int32_t val = PIOS_HMC5x83_Config(cfg); + + PIOS_Assert(val == 0); + + pios_hmc5x83_data_ready = false; +} + +/** + * @brief Initialize the HMC5x83 magnetometer sensor + * \return none + * \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor. + * + * CTRL_REGA: Control Register A + * Read Write + * Default value: 0x10 + * 7:5 0 These bits must be cleared for correct operation. + * 4:2 DO2-DO0: Data Output Rate Bits + * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) + * ------------------------------------------------------ + * 0 | 0 | 0 | 0.75 + * 0 | 0 | 1 | 1.5 + * 0 | 1 | 0 | 3 + * 0 | 1 | 1 | 7.5 + * 1 | 0 | 0 | 15 (default) + * 1 | 0 | 1 | 30 + * 1 | 1 | 0 | 75 + * 1 | 1 | 1 | Not Used + * 1:0 MS1-MS0: Measurement Configuration Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Normal + * 0 | 1 | Positive Bias + * 1 | 0 | Negative Bias + * 1 | 1 | Not Used + * + * CTRL_REGB: Control RegisterB + * Read Write + * Default value: 0x20 + * 7:5 GN2-GN0: Gain Configuration Bits. + * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range + * | | | Range[Ga] | [LSB/mGa] | + * ------------------------------------------------------ + * 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047) + * 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047) + * |Not recommended| + * + * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. + * + * _MODE_REG: Mode Register + * Read Write + * Default value: 0x02 + * 7:2 0 These bits must be cleared for correct operation. + * 1:0 MD1-MD0: Mode Select Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Continuous-Conversion Mode. + * 0 | 1 | Single-Conversion Mode + * 1 | 0 | Negative Bias + * 1 | 1 | Sleep Mode + */ +static uint8_t CTRLB = 0x00; +static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) +{ + uint8_t CTRLA = 0x00; + uint8_t MODE = 0x00; + + CTRLB = 0; + + CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLB |= (uint8_t)(cfg->Gain); + MODE |= (uint8_t)(cfg->Mode); + + // CRTL_REGA + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { + return -1; + } + + // CRTL_REGB + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, CTRLB) != 0) { + return -1; + } + + // Mode register + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, MODE) != 0) { + return -1; + } + + return 0; +} + +/** + * @brief Read current X, Z, Y values (in that order) + * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings + * \return 0 for success or -1 for failure + */ +int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) +{ + pios_hmc5x83_data_ready = false; + uint8_t buffer[6]; + int32_t temp; + int32_t sensitivity; + + if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { + return -1; + } + + switch (CTRLB & 0xE0) { + case 0x00: + sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga; + break; + case 0x20: + sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga; + break; + case 0x40: + sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga; + break; + case 0x60: + sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga; + break; + case 0x80: + sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga; + break; + case 0xA0: + sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga; + break; + case 0xC0: + sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga; + break; + case 0xE0: + sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga; + break; + default: + PIOS_Assert(0); + } + + for (int i = 0; i < 3; i++) { + temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / sensitivity; + out[i] = temp; + } + // Data reads out as X,Z,Y + temp = out[2]; + out[2] = out[1]; + out[1] = temp; + + // This should not be necessary but for some reason it is coming out of continuous conversion mode + dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); + + return 0; +} + + +/** + * @brief Read the identification bytes from the HMC5x83 sensor + * \param[out] uint8_t array of size 4 to store HMC5x83 ID. + * \return 0 if successful, -1 if not + */ +uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) +{ + uint8_t retval = dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); + + out[3] = '\0'; + return retval; +} + +/** + * @brief Tells whether new magnetometer readings are available + * \return true if new data is available + * \return false if new data is not available + */ +bool PIOS_HMC5x83_NewDataAvailable(void) +{ + return pios_hmc5x83_data_ready; +} + +/** + * @brief Run self-test operation. Do not call this during operational use!! + * \return 0 if success, -1 if test failed + */ +int32_t PIOS_HMC5x83_Test(void) +{ + int32_t failed = 0; + uint8_t registers[3] = { 0, 0, 0 }; + uint8_t status; + uint8_t ctrl_a_read; + uint8_t ctrl_b_read; + uint8_t mode_read; + int16_t values[3]; + + + /* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */ + char id[4]; + + PIOS_HMC5x83_ReadID((uint8_t *)id); + if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 + return -1; + } + + /* Backup existing configuration */ + if (dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { + return -1; + } + + /* Stop the device and read out last value */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { + return -1; + } + if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { + return -1; + } + if (PIOS_HMC5x83_ReadMag(values) != 0) { + return -1; + } + + /* + * Put HMC5x83 into self test mode + * This is done by placing measurement config into positive (0x01) or negative (0x10) bias + * and then placing the mode register into single-measurement mode. This causes the HMC5x83 + * to create an artificial magnetic field of ~1.1 Gauss. + * + * If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB + * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) + * + * Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode. + */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { + return -1; + } + + /* Must wait for value to be updated */ + PIOS_DELAY_WaitmS(200); + + if (PIOS_HMC5x83_ReadMag(values) != 0) { + return -1; + } + + /* + if(abs(values[0] - 766) > 20) + failed |= 1; + if(abs(values[1] - 766) > 20) + failed |= 1; + if(abs(values[2] - 713) > 20) + failed |= 1; + */ + + dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_MODE_REG, &mode_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); + + + /* Restore backup configuration */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { + return -1; + } + + return failed; +} + +/** + * @brief IRQ Handler + */ +bool PIOS_HMC5x83_IRQHandler(void) +{ + pios_hmc5x83_data_ready = true; + + return false; +} + +#ifdef PIOS_INCLUDE_SPI +int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer); + +const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = { + .Read = PIOS_HMC5x83_SPI_Read, + .Write = PIOS_HMC5x83_SPI_Write, +}; + +static int32_t pios_hmc5x83_spi_claim_bus() +{ + if (PIOS_SPI_ClaimBus(dev_port_id) < 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 0); + return 0; +} + +static void pios_hmc5x83_spi_release_bus() +{ + PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 1); + PIOS_SPI_ReleaseBus(dev_port_id); +} +/** + * @brief Reads one or more bytes into a buffer + * \param[in] address HMC5x83 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) +{ + if (pios_hmc5x83_spi_claim_bus() < 0) { + return -1; + } + + memset(buffer, 0xA5, len); + PIOS_SPI_TransferByte(dev_port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); + + // buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG; + /* Copy the transfer data to the buffer */ + if (PIOS_SPI_TransferBlock(dev_port_id, NULL, buffer, len, NULL) < 0) { + pios_hmc5x83_spi_release_bus(); + return -3; + } + pios_hmc5x83_spi_release_bus(); + return 0; +} + +/** + * @brief Writes one or more bytes to the HMC5x83 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim spi device + */ +int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) +{ + if (pios_hmc5x83_spi_claim_bus() < 0) { + return -1; + } + uint8_t data[] = { + address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG, + buffer, + }; + + if (PIOS_SPI_TransferBlock(dev_port_id, data, NULL, sizeof(data), NULL) < 0) { + pios_hmc5x83_spi_release_bus(); + return -2; + } + + pios_hmc5x83_spi_release_bus(); + return 0; +} +#endif /* ifdef PIOS_INCLUDE_SPI */ +#ifdef PIOS_INCLUDE_I2C + +int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer); + +const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { + .Read = PIOS_HMC5x83_I2C_Read, + .Write = PIOS_HMC5x83_I2C_Write, +}; + +/** + * @brief Reads one or more bytes into a buffer + * \param[in] address HMC5x83 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +{ + uint8_t addr_buffer[] = { + address, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); +} + +/** + * @brief Writes one or more bytes to the HMC5x83 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) +{ + uint8_t data[] = { + address, + buffer, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; + + ; + return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); +} +#endif /* ifdef PIOS_INCLUDE_I2C */ + + +#endif /* PIOS_INCLUDE_HMC5x83 */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h new file mode 100644 index 000000000..fc0d9bbab --- /dev/null +++ b/flight/pios/inc/pios_hmc5x83.h @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HMC5x83 HMC5x83 Functions + * @brief Deals with the hardware interface to the magnetometers + * @{ + * + * @file pios_hmc5x83.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief HMC5x83 functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 PIOS_HMC5x83_H +#define PIOS_HMC5x83_H + +/* HMC5x83 Addresses */ +#define PIOS_HMC5x83_I2C_ADDR 0x1E +#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D +#define PIOS_HMC5x83_I2C_WRITE_ADDR 0x3C + +#define PIOS_HMC5x83_SPI_READ_FLAG 0x80 +#define PIOS_HMC5x83_SPI_AUTOINCR_FLAG 0x40 +#define PIOS_HMC5x83_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5x83_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5x83_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5x83_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5x83_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5x83_DATAOUT_ZMSB_REG 0x05 +#define PIOS_HMC5x83_DATAOUT_ZLSB_REG 0x06 +#define PIOS_HMC5x83_DATAOUT_YMSB_REG 0x07 +#define PIOS_HMC5x83_DATAOUT_YLSB_REG 0x08 +#define PIOS_HMC5x83_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5x83_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5x83_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5x83_DATAOUT_IDC_REG 0x0C + +/* Output Data Rate */ +#define PIOS_HMC5x83_ODR_0_75 0x00 +#define PIOS_HMC5x83_ODR_1_5 0x04 +#define PIOS_HMC5x83_ODR_3 0x08 +#define PIOS_HMC5x83_ODR_7_5 0x0C +#define PIOS_HMC5x83_ODR_15 0x10 +#define PIOS_HMC5x83_ODR_30 0x14 +#define PIOS_HMC5x83_ODR_75 0x18 + +/* Measure configuration */ +#define PIOS_HMC5x83_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5x83_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5x83_MEASCONF_BIAS_NEG 0x02 + +/* Gain settings */ +#define PIOS_HMC5x83_GAIN_0_88 0x00 +#define PIOS_HMC5x83_GAIN_1_3 0x20 +#define PIOS_HMC5x83_GAIN_1_9 0x40 +#define PIOS_HMC5x83_GAIN_2_5 0x60 +#define PIOS_HMC5x83_GAIN_4_0 0x80 +#define PIOS_HMC5x83_GAIN_4_7 0xA0 +#define PIOS_HMC5x83_GAIN_5_6 0xC0 +#define PIOS_HMC5x83_GAIN_8_1 0xE0 + +/* Modes */ +#define PIOS_HMC5x83_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5x83_MODE_SINGLE 0x01 +#define PIOS_HMC5x83_MODE_IDLE 0x02 +#define PIOS_HMC5x83_MODE_SLEEP 0x03 + +/* Sensitivity Conversion Values */ +#define PIOS_HMC5x83_Sensitivity_0_88Ga 1370 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_1_3Ga 1090 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_1_9Ga 820 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_2_5Ga 660 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_4_0Ga 440 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_4_7Ga 390 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED + +struct pios_hmc5x83_io_driver { + int32_t (*Write)(uint8_t address, uint8_t buffer); + int32_t (*Read)(uint8_t address, uint8_t *buffer, uint8_t len); +}; + +#ifdef PIOS_INCLUDE_SPI +extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER; +#endif + +#ifdef PIOS_INCLUDE_I2C +extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER; +#endif + +struct pios_hmc5x83_cfg { +#ifdef PIOS_HMC5x83_HAS_GPIOS + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ +#endif + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; + const struct pios_hmc5x83_io_driver *Driver; +}; + +/* Public Functions */ +extern void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); +extern bool PIOS_HMC5x83_NewDataAvailable(void); +extern int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]); +extern uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]); +extern int32_t PIOS_HMC5x83_Test(void); +extern bool PIOS_HMC5x83_IRQHandler(); + +#endif /* PIOS_HMC5x83_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 60535ede9..1bb1ea968 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -210,6 +210,12 @@ #include #endif +#ifdef PIOS_INCLUDE_HMC5X83 +/* HMC5883/HMC5983 3-Axis Digital Compass */ +/* #define PIOS_HMC5x83_HAS_GPIOS */ +#include +#endif + #ifdef PIOS_INCLUDE_BMP085 /* BMP085 Barometric Pressure Sensor */ #include From 2a8429320d0f6a055e5dfe74948dac82a804b373 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 12 Jul 2014 12:03:10 +0200 Subject: [PATCH 089/718] OP-1403 - Support HMC5983 temperature sensor/compensation --- flight/pios/common/pios_hmc5x83.c | 1 + flight/pios/inc/pios_hmc5x83.h | 9 ++++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 922b5fde7..d1854632f 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -135,6 +135,7 @@ static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) CTRLB = 0; CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0; CTRLB |= (uint8_t)(cfg->Gain); MODE |= (uint8_t)(cfg->Mode); diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index fc0d9bbab..94f53b2f9 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -76,6 +76,8 @@ #define PIOS_HMC5x83_GAIN_5_6 0xC0 #define PIOS_HMC5x83_GAIN_8_1 0xE0 +#define PIOS_HMC5x83_CTRLA_TEMP 0x40 + /* Modes */ #define PIOS_HMC5x83_MODE_CONTINUOUS 0x00 #define PIOS_HMC5x83_MODE_SINGLE 0x01 @@ -109,10 +111,11 @@ struct pios_hmc5x83_cfg { #ifdef PIOS_HMC5x83_HAS_GPIOS const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; // Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ uint8_t Mode; + bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation const struct pios_hmc5x83_io_driver *Driver; }; From 57a43a5587891565c27452d3d7c2649583cd8d64 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 18 Jul 2014 20:34:20 +0200 Subject: [PATCH 090/718] OP-1403 - Migrate to new HMC5x83 driver and remove old HMC5883 one --- flight/modules/Attitude/revolution/attitude.c | 2 +- flight/modules/Extensions/MagBaro/magbaro.c | 29 +- flight/modules/Sensors/sensors.c | 12 +- flight/modules/StateEstimation/filtercf.c | 2 +- flight/pios/common/pios_hmc5883.c | 425 ------------------ flight/pios/inc/pios_hmc5883.h | 116 ----- flight/pios/inc/pios_hmc5x83.h | 2 +- flight/pios/pios.h | 6 - .../coptercontrol/firmware/inc/pios_config.h | 2 +- .../firmware/inc/pios_config.h | 4 +- .../discoveryf4bare/firmware/pios_board.c | 27 +- .../oplinkmini/firmware/inc/pios_config.h | 2 +- .../boards/osd/firmware/inc/pios_config.h | 4 +- .../revolution/firmware/inc/pios_config.h | 4 +- .../boards/revolution/firmware/pios_board.c | 27 +- .../revoproto/firmware/inc/pios_config.h | 4 +- .../boards/revoproto/firmware/pios_board.c | 27 +- .../simposix/firmware/inc/pios_config.h | 2 +- make/apps-defs.mk | 2 +- 19 files changed, 78 insertions(+), 621 deletions(-) delete mode 100644 flight/pios/common/pios_hmc5883.c delete mode 100644 flight/pios/inc/pios_hmc5883.h diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index b49ac751b..2ee8901e0 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -352,7 +352,7 @@ static int32_t updateAttitudeComplementary(bool first_run) // During initialization and if (first_run) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) // To initialize we need a valid mag reading if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { return -1; diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 2b219ee16..aa956c57e 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -64,7 +64,7 @@ static int32_t alt_ds_pres = 0; static int alt_ds_count = 0; #endif -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) int32_t mag_test; static float mag_bias[3] = { 0, 0, 0 }; static float mag_scale[3] = { 1, 1, 1 }; @@ -108,7 +108,7 @@ int32_t MagBaroInitialize() #endif if (magbaroEnabled) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorInitialize(); #endif @@ -127,15 +127,16 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart); /** * Module thread, should not return. */ -#if defined(PIOS_INCLUDE_HMC5883) -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { -#ifdef PIOS_HMC5883_HAS_GPIOS +#if defined(PIOS_INCLUDE_HMC5X83) +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS .exti_cfg = 0, #endif - .M_ODR = PIOS_HMC5883_ODR_15, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, + .M_ODR = PIOS_HMC5x83_ODR_15, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; #endif @@ -148,9 +149,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters) PIOS_BMP085_Init(); #endif -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - PIOS_HMC5883_Init(&pios_hmc5883_cfg); + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -197,10 +198,10 @@ static void magbaroTask(__attribute__((unused)) void *parameters) } #endif /* if defined(PIOS_INCLUDE_BMP085) */ -#if defined(PIOS_INCLUDE_HMC5883) - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { +#if defined(PIOS_INCLUDE_HMC5X83) + if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { int16_t values[3]; - PIOS_HMC5883_ReadMag(values); + PIOS_HMC5x83_ReadMag(values); float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], (float)values[0] * mag_scale[1] - mag_bias[1], -(float)values[2] * mag_scale[2] - mag_bias[2] }; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index e1d85b2f1..c93dc64f8 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -200,8 +200,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters) PIOS_DEBUG_Assert(0); } -#if defined(PIOS_INCLUDE_HMC5883) - mag_test = PIOS_HMC5883_Test(); +#if defined(PIOS_INCLUDE_HMC5X83) + mag_test = PIOS_HMC5x83_Test(); #else mag_test = 0; #endif @@ -409,11 +409,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters) // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; - PIOS_HMC5883_ReadMag(values); + PIOS_HMC5x83_ReadMag(values); float mags[3] = { (float)values[1] - mag_bias[0], (float)values[0] - mag_bias[1], -(float)values[2] - mag_bias[2] }; @@ -428,7 +428,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } -#endif /* if defined(PIOS_INCLUDE_HMC5883) */ +#endif /* if defined(PIOS_INCLUDE_HMC5X83) */ #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 15b61a665..d2e109655 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -221,7 +221,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // During initialization and if (this->first_run) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) // wait until mags have been updated if (!this->magUpdated) { return FILTERRESULT_ERROR; diff --git a/flight/pios/common/pios_hmc5883.c b/flight/pios/common/pios_hmc5883.c deleted file mode 100644 index d425e16dc..000000000 --- a/flight/pios/common/pios_hmc5883.c +++ /dev/null @@ -1,425 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HMC5883 HMC5883 Functions - * @brief Deals with the hardware interface to the magnetometers - * @{ - * @file pios_hmc5883.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief HMC5883 Magnetic Sensor Functions from AHRS - * @see The GNU Public License (GPL) Version 3 - * - ****************************************************************************** - */ -/* - * 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 "pios.h" - -#ifdef PIOS_INCLUDE_HMC5883 - -/* Global Variables */ - -/* Local Types */ - -/* Local Variables */ -volatile bool pios_hmc5883_data_ready; - -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg); -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len); -static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer); - -static const struct pios_hmc5883_cfg *dev_cfg; - -/** - * @brief Initialize the HMC5883 magnetometer sensor. - * @return none - */ -void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg) -{ - dev_cfg = cfg; // store config before enabling interrupt - -#ifdef PIOS_HMC5883_HAS_GPIOS - PIOS_EXTI_Init(cfg->exti_cfg); -#endif - - int32_t val = PIOS_HMC5883_Config(cfg); - - PIOS_Assert(val == 0); - - pios_hmc5883_data_ready = false; -} - -/** - * @brief Initialize the HMC5883 magnetometer sensor - * \return none - * \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor. - * - * CTRL_REGA: Control Register A - * Read Write - * Default value: 0x10 - * 7:5 0 These bits must be cleared for correct operation. - * 4:2 DO2-DO0: Data Output Rate Bits - * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) - * ------------------------------------------------------ - * 0 | 0 | 0 | 0.75 - * 0 | 0 | 1 | 1.5 - * 0 | 1 | 0 | 3 - * 0 | 1 | 1 | 7.5 - * 1 | 0 | 0 | 15 (default) - * 1 | 0 | 1 | 30 - * 1 | 1 | 0 | 75 - * 1 | 1 | 1 | Not Used - * 1:0 MS1-MS0: Measurement Configuration Bits - * MS1 | MS0 | MODE - * ------------------------------ - * 0 | 0 | Normal - * 0 | 1 | Positive Bias - * 1 | 0 | Negative Bias - * 1 | 1 | Not Used - * - * CTRL_REGB: Control RegisterB - * Read Write - * Default value: 0x20 - * 7:5 GN2-GN0: Gain Configuration Bits. - * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range - * | | | Range[Ga] | [LSB/mGa] | - * ------------------------------------------------------ - * 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047) - * 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047) - * 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047) - * 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047) - * 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047) - * 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047) - * 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047) - * 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047) - * |Not recommended| - * - * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. - * - * _MODE_REG: Mode Register - * Read Write - * Default value: 0x02 - * 7:2 0 These bits must be cleared for correct operation. - * 1:0 MD1-MD0: Mode Select Bits - * MS1 | MS0 | MODE - * ------------------------------ - * 0 | 0 | Continuous-Conversion Mode. - * 0 | 1 | Single-Conversion Mode - * 1 | 0 | Negative Bias - * 1 | 1 | Sleep Mode - */ -static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg) -{ - uint8_t CTRLA = 0x00; - uint8_t MODE = 0x00; - - CTRLB = 0; - - CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); - CTRLB |= (uint8_t)(cfg->Gain); - MODE |= (uint8_t)(cfg->Mode); - - // CRTL_REGA - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) { - return -1; - } - - // CRTL_REGB - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) { - return -1; - } - - // Mode register - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) { - return -1; - } - - return 0; -} - -/** - * @brief Read current X, Z, Y values (in that order) - * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings - * \return 0 for success or -1 for failure - */ -int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) -{ - pios_hmc5883_data_ready = false; - uint8_t buffer[6]; - int32_t temp; - int32_t sensitivity; - - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { - return -1; - } - - switch (CTRLB & 0xE0) { - case 0x00: - sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; - break; - case 0x20: - sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; - break; - case 0x40: - sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; - break; - case 0x60: - sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; - break; - case 0x80: - sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; - break; - case 0xA0: - sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; - break; - case 0xC0: - sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; - break; - case 0xE0: - sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; - break; - default: - PIOS_Assert(0); - } - - for (int i = 0; i < 3; i++) { - temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / sensitivity; - out[i] = temp; - } - // Data reads out as X,Z,Y - temp = out[2]; - out[2] = out[1]; - out[1] = temp; - - // This should not be necessary but for some reason it is coming out of continuous conversion mode - PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); - - return 0; -} - - -/** - * @brief Read the identification bytes from the HMC5883 sensor - * \param[out] uint8_t array of size 4 to store HMC5883 ID. - * \return 0 if successful, -1 if not - */ -uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) -{ - uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); - - out[3] = '\0'; - return retval; -} - -/** - * @brief Tells whether new magnetometer readings are available - * \return true if new data is available - * \return false if new data is not available - */ -bool PIOS_HMC5883_NewDataAvailable(void) -{ - return pios_hmc5883_data_ready; -} - -/** - * @brief Reads one or more bytes into a buffer - * \param[in] address HMC5883 register address (depends on size) - * \param[out] buffer destination buffer - * \param[in] len number of bytes which should be read - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - * \return -2 if unable to claim i2c device - */ -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len) -{ - uint8_t addr_buffer[] = { - address, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); -} - -/** - * @brief Writes one or more bytes to the HMC5883 - * \param[in] address Register address - * \param[in] buffer source buffer - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - * \return -2 if unable to claim i2c device - */ -static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) -{ - uint8_t data[] = { - address, - buffer, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; - - ; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); -} - -/** - * @brief Run self-test operation. Do not call this during operational use!! - * \return 0 if success, -1 if test failed - */ -int32_t PIOS_HMC5883_Test(void) -{ - int32_t failed = 0; - uint8_t registers[3] = { 0, 0, 0 }; - uint8_t status; - uint8_t ctrl_a_read; - uint8_t ctrl_b_read; - uint8_t mode_read; - int16_t values[3]; - - - /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ - char id[4]; - - PIOS_HMC5883_ReadID((uint8_t *)id); - if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 - return -1; - } - - /* Backup existing configuration */ - if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) { - return -1; - } - - /* Stop the device and read out last value */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) { - return -1; - } - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) { - return -1; - } - if (PIOS_HMC5883_ReadMag(values) != 0) { - return -1; - } - - /* - * Put HMC5883 into self test mode - * This is done by placing measurement config into positive (0x01) or negative (0x10) bias - * and then placing the mode register into single-measurement mode. This causes the HMC5883 - * to create an artificial magnetic field of ~1.1 Gauss. - * - * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB - * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) - * - * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. - */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) { - return -1; - } - - /* Must wait for value to be updated */ - PIOS_DELAY_WaitmS(200); - - if (PIOS_HMC5883_ReadMag(values) != 0) { - return -1; - } - - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1); - - - /* Restore backup configuration */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) { - return -1; - } - - return failed; -} - -/** - * @brief IRQ Handler - */ -bool PIOS_HMC5883_IRQHandler(void) -{ - pios_hmc5883_data_ready = true; - - return false; -} - -#endif /* PIOS_INCLUDE_HMC5883 */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_hmc5883.h b/flight/pios/inc/pios_hmc5883.h deleted file mode 100644 index 6d34dae20..000000000 --- a/flight/pios/inc/pios_hmc5883.h +++ /dev/null @@ -1,116 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HMC5883 HMC5883 Functions - * @brief Deals with the hardware interface to the magnetometers - * @{ - * - * @file pios_hmc5883.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief HMC5883 functions header. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * 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 PIOS_HMC5883_H -#define PIOS_HMC5883_H - -/* HMC5883 Addresses */ -#define PIOS_HMC5883_I2C_ADDR 0x1E -#define PIOS_HMC5883_I2C_READ_ADDR 0x3D -#define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C -#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 -#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 -#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 -#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 -#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C - -/* Output Data Rate */ -#define PIOS_HMC5883_ODR_0_75 0x00 -#define PIOS_HMC5883_ODR_1_5 0x04 -#define PIOS_HMC5883_ODR_3 0x08 -#define PIOS_HMC5883_ODR_7_5 0x0C -#define PIOS_HMC5883_ODR_15 0x10 -#define PIOS_HMC5883_ODR_30 0x14 -#define PIOS_HMC5883_ODR_75 0x18 - -/* Measure configuration */ -#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 - -/* Gain settings */ -#define PIOS_HMC5883_GAIN_0_88 0x00 -#define PIOS_HMC5883_GAIN_1_3 0x20 -#define PIOS_HMC5883_GAIN_1_9 0x40 -#define PIOS_HMC5883_GAIN_2_5 0x60 -#define PIOS_HMC5883_GAIN_4_0 0x80 -#define PIOS_HMC5883_GAIN_4_7 0xA0 -#define PIOS_HMC5883_GAIN_5_6 0xC0 -#define PIOS_HMC5883_GAIN_8_1 0xE0 - -/* Modes */ -#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5883_MODE_SINGLE 0x01 -#define PIOS_HMC5883_MODE_IDLE 0x02 -#define PIOS_HMC5883_MODE_SLEEP 0x03 - -/* Sensitivity Conversion Values */ -#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED - - -struct pios_hmc5883_cfg { -#ifdef PIOS_HMC5883_HAS_GPIOS - const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ -#endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; -}; - -/* Public Functions */ -extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg); -extern bool PIOS_HMC5883_NewDataAvailable(void); -extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); -extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); -extern int32_t PIOS_HMC5883_Test(void); -extern bool PIOS_HMC5883_IRQHandler(); - -#endif /* PIOS_HMC5883_H */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 94f53b2f9..949966ef5 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -108,7 +108,7 @@ extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER; #endif struct pios_hmc5x83_cfg { -#ifdef PIOS_HMC5x83_HAS_GPIOS +#ifdef PIOS_HMC5X83_HAS_GPIOS const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 1bb1ea968..124dc1d8a 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -204,12 +204,6 @@ #include #endif -#ifdef PIOS_INCLUDE_HMC5883 -/* HMC5883 3-Axis Digital Compass */ -/* #define PIOS_HMC5883_HAS_GPIOS */ -#include -#endif - #ifdef PIOS_INCLUDE_HMC5X83 /* HMC5883/HMC5983 3-Axis Digital Compass */ /* #define PIOS_HMC5x83_HAS_GPIOS */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index e7da296e9..95332df45 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -/* #define PIOS_INCLUDE_HMC5883 */ +/* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index a55d4cb09..44065be33 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -84,8 +84,8 @@ // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -// #define PIOS_INCLUDE_HMC5883 -// #define PIOS_HMC5883_HAS_GPIOS +// #define PIOS_INCLUDE_HMC5X83 +// #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ // #define PIOS_INCLUDE_MS5611 // #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 687f1c5f7..2e8971636 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -90,10 +90,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -123,14 +123,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -929,8 +930,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 8dd30c0f3..7717beb52 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -81,7 +81,7 @@ /* #define PIOS_INCLUDE_MPU6000 */ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ -/* #define PIOS_INCLUDE_HMC5883 */ +/* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index 33542a953..66e178349 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -81,8 +81,8 @@ /* #define PIOS_INCLUDE_MPU6000 */ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -/* #define PIOS_HMC5883_HAS_GPIOS */ +#define PIOS_INCLUDE_HMC5X83 +/* #define PIOS_HMC5X83_HAS_GPIOS */ #define PIOS_INCLUDE_BMP085 /* #define PIOS_INCLUDE_MS5611 */ /* #define PIOS_INCLUDE_MPXV */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 49cd0d8e6..5ee827783 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -84,8 +84,8 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -#define PIOS_HMC5883_HAS_GPIOS +#define PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 4901cc432..b97adb07a 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -91,10 +91,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -124,14 +124,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -944,8 +945,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 6077225aa..897f9b360 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -81,8 +81,8 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -#define PIOS_HMC5883_HAS_GPIOS +#define PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index e628d615e..de2c5982d 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -81,10 +81,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line5, .pin = { .gpio = GPIOB, @@ -114,14 +114,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -938,8 +939,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 9194e68b9..0dc6b7aaa 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -63,7 +63,7 @@ /* Select the sensors to include */ // #define PIOS_INCLUDE_BMA180 -// #define PIOS_INCLUDE_HMC5883 +// #define PIOS_INCLUDE_HMC5X83 // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL // #define PIOS_INCLUDE_L3GD20 diff --git a/make/apps-defs.mk b/make/apps-defs.mk index a80fc6fc5..72381423e 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -64,7 +64,7 @@ SRC += $(PIOSCOMMON)/pios_etasv3.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_hcsr04.c SRC += $(PIOSCOMMON)/pios_hmc5843.c -SRC += $(PIOSCOMMON)/pios_hmc5883.c +SRC += $(PIOSCOMMON)/pios_hmc5x83.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_mpu6000.c From 6a3cb7b808177386519158e87d83675fd68deefc Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:22:44 +0200 Subject: [PATCH 091/718] OP-1403 - Add support for multiple hmc* devices --- flight/pios/common/pios_hmc5x83.c | 212 ++++++++++++++++++------------ flight/pios/inc/pios_hmc5x83.h | 20 +-- 2 files changed, 136 insertions(+), 96 deletions(-) diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index d1854632f..73b4902d9 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -30,46 +30,79 @@ #include "pios.h" #include +#include #ifdef PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_MAGIC 0x4d783833 /* Global Variables */ /* Local Types */ -/* Local Variables */ -volatile bool pios_hmc5x83_data_ready; +typedef struct { + uint32_t magic; + const struct pios_hmc5x83_cfg *cfg; + uint32_t port_id; + uint8_t slave_num; + uint8_t CTRLB; + volatile bool data_ready; +} pios_hmc5x83_dev_data_t; -static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg); -// static int32_t PIOS_HMC5x83_Read(uint8_t address, uint8_t *buffer, uint8_t len); -// static int32_t PIOS_HMC5x83_Write(uint8_t address, uint8_t buffer); +static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev); + +/** + * Allocate the device setting structure + * @return pios_hmc5x83_dev_data_t pointer to newly created structure + */ +pios_hmc5x83_dev_data_t *dev_alloc() +{ + pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t)); + + PIOS_DEBUG_Assert(dev); + memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t)); + dev->magic = PIOS_HMC5X83_MAGIC; + return dev; +} + +/** + * Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer + * @param dev device handler + * @return the device data structure + */ +pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev) +{ + pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev; + + PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC); + return dev_data; +} -static const struct pios_hmc5x83_cfg *dev_cfg; -static uint32_t dev_port_id; -static uint8_t dev_slave_num; /** * @brief Initialize the HMC5x83 magnetometer sensor. * @return none */ -void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) +pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) { - dev_cfg = cfg; // store config before enabling interrupt - dev_port_id = port_id; - dev_slave_num = slave_num; + pios_hmc5x83_dev_data_t *dev = dev_alloc(); + + dev->cfg = cfg; // store config before enabling interrupt + dev->port_id = port_id; + dev->slave_num = slave_num; #ifdef PIOS_HMC5X83_HAS_GPIOS PIOS_EXTI_Init(cfg->exti_cfg); #endif - int32_t val = PIOS_HMC5x83_Config(cfg); - + int32_t val = PIOS_HMC5x83_Config(dev); PIOS_Assert(val == 0); - pios_hmc5x83_data_ready = false; + dev->data_ready = false; + return (pios_hmc5x83_dev_t)dev; } /** * @brief Initialize the HMC5x83 magnetometer sensor * \return none + * \param[in] pios_hmc5x83_dev_data_t device config to be used. * \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor. * * CTRL_REGA: Control Register A @@ -126,31 +159,32 @@ void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uin * 1 | 0 | Negative Bias * 1 | 1 | Sleep Mode */ -static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) +static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev) { uint8_t CTRLA = 0x00; uint8_t MODE = 0x00; - CTRLB = 0; + const struct pios_hmc5x83_cfg *cfg = dev->cfg; + + dev->CTRLB = 0; CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0; - CTRLB |= (uint8_t)(cfg->Gain); + dev->CTRLB |= (uint8_t)(cfg->Gain); MODE |= (uint8_t)(cfg->Mode); // CRTL_REGA - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { return -1; } // CRTL_REGB - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, CTRLB) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) { return -1; } // Mode register - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, MODE) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) { return -1; } @@ -159,21 +193,24 @@ static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) /** * @brief Read current X, Z, Y values (in that order) + * \param[in] dev device handler * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings * \return 0 for success or -1 for failure */ -int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) +int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]) { - pios_hmc5x83_data_ready = false; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + dev->data_ready = false; uint8_t buffer[6]; int32_t temp; int32_t sensitivity; - if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { return -1; } - switch (CTRLB & 0xE0) { + switch (dev->CTRLB & 0xE0) { case 0x00: sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga; break; @@ -213,7 +250,7 @@ int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) out[1] = temp; // This should not be necessary but for some reason it is coming out of continuous conversion mode - dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); + dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); return 0; } @@ -224,9 +261,10 @@ int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) * \param[out] uint8_t array of size 4 to store HMC5x83 ID. * \return 0 if successful, -1 if not */ -uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) +uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]) { - uint8_t retval = dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); out[3] = '\0'; return retval; @@ -237,16 +275,18 @@ uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) * \return true if new data is available * \return false if new data is not available */ -bool PIOS_HMC5x83_NewDataAvailable(void) +bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler) { - return pios_hmc5x83_data_ready; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + return dev->data_ready; } /** * @brief Run self-test operation. Do not call this during operational use!! * \return 0 if success, -1 if test failed */ -int32_t PIOS_HMC5x83_Test(void) +int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler) { int32_t failed = 0; uint8_t registers[3] = { 0, 0, 0 }; @@ -255,30 +295,30 @@ int32_t PIOS_HMC5x83_Test(void) uint8_t ctrl_b_read; uint8_t mode_read; int16_t values[3]; - + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); /* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */ char id[4]; - PIOS_HMC5x83_ReadID((uint8_t *)id); + PIOS_HMC5x83_ReadID(handler, (uint8_t *)id); if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 return -1; } /* Backup existing configuration */ - if (dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { return -1; } /* Stop the device and read out last value */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { return -1; } - if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { return -1; } - if (PIOS_HMC5x83_ReadMag(values) != 0) { + if (PIOS_HMC5x83_ReadMag(handler, values) != 0) { return -1; } @@ -294,51 +334,42 @@ int32_t PIOS_HMC5x83_Test(void) * Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode. */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { return -1; } /* Must wait for value to be updated */ PIOS_DELAY_WaitmS(200); - if (PIOS_HMC5x83_ReadMag(values) != 0) { + if (PIOS_HMC5x83_ReadMag(handler, values) != 0) { return -1; } - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_MODE_REG, &mode_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); /* Restore backup configuration */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { return -1; } @@ -348,35 +379,36 @@ int32_t PIOS_HMC5x83_Test(void) /** * @brief IRQ Handler */ -bool PIOS_HMC5x83_IRQHandler(void) +bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler) { - pios_hmc5x83_data_ready = true; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + dev->data_ready = true; return false; } #ifdef PIOS_INCLUDE_SPI -int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len); -int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer); +int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = { .Read = PIOS_HMC5x83_SPI_Read, .Write = PIOS_HMC5x83_SPI_Write, }; -static int32_t pios_hmc5x83_spi_claim_bus() +static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev) { - if (PIOS_SPI_ClaimBus(dev_port_id) < 0) { + if (PIOS_SPI_ClaimBus(dev->port_id) < 0) { return -1; } - PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 0); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0); return 0; } -static void pios_hmc5x83_spi_release_bus() +static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev) { - PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 1); - PIOS_SPI_ReleaseBus(dev_port_id); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1); + PIOS_SPI_ReleaseBus(dev->port_id); } /** * @brief Reads one or more bytes into a buffer @@ -387,22 +419,24 @@ static void pios_hmc5x83_spi_release_bus() * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) +int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len) { - if (pios_hmc5x83_spi_claim_bus() < 0) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + if (pios_hmc5x83_spi_claim_bus(dev) < 0) { return -1; } memset(buffer, 0xA5, len); - PIOS_SPI_TransferByte(dev_port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); + PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); // buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG; /* Copy the transfer data to the buffer */ - if (PIOS_SPI_TransferBlock(dev_port_id, NULL, buffer, len, NULL) < 0) { - pios_hmc5x83_spi_release_bus(); + if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) { + pios_hmc5x83_spi_release_bus(dev); return -3; } - pios_hmc5x83_spi_release_bus(); + pios_hmc5x83_spi_release_bus(dev); return 0; } @@ -414,9 +448,11 @@ int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) * \return -1 if error during I2C transfer * \return -2 if unable to claim spi device */ -int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) +int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer) { - if (pios_hmc5x83_spi_claim_bus() < 0) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + if (pios_hmc5x83_spi_claim_bus(dev) < 0) { return -1; } uint8_t data[] = { @@ -424,19 +460,19 @@ int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) buffer, }; - if (PIOS_SPI_TransferBlock(dev_port_id, data, NULL, sizeof(data), NULL) < 0) { - pios_hmc5x83_spi_release_bus(); + if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) { + pios_hmc5x83_spi_release_bus(dev); return -2; } - pios_hmc5x83_spi_release_bus(); + pios_hmc5x83_spi_release_bus(dev); return 0; } -#endif /* ifdef PIOS_INCLUDE_SPI */ +#endif /* PIOS_INCLUDE_SPI */ #ifdef PIOS_INCLUDE_I2C -int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len); -int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer); +int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { .Read = PIOS_HMC5x83_I2C_Read, @@ -452,8 +488,9 @@ const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); uint8_t addr_buffer[] = { address, }; @@ -476,7 +513,7 @@ int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) } }; - return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); } /** @@ -487,8 +524,9 @@ int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) +int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); uint8_t data[] = { address, buffer, @@ -506,9 +544,9 @@ int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) }; ; - return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); } -#endif /* ifdef PIOS_INCLUDE_I2C */ +#endif /* PIOS_INCLUDE_I2C */ #endif /* PIOS_INCLUDE_HMC5x83 */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 949966ef5..dc15c35bf 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -30,7 +30,7 @@ #ifndef PIOS_HMC5x83_H #define PIOS_HMC5x83_H - +#include /* HMC5x83 Addresses */ #define PIOS_HMC5x83_I2C_ADDR 0x1E #define PIOS_HMC5x83_I2C_READ_ADDR 0x3D @@ -94,9 +94,11 @@ #define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga #define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED +typedef uintptr_t pios_hmc5x83_dev_t; + struct pios_hmc5x83_io_driver { - int32_t (*Write)(uint8_t address, uint8_t buffer); - int32_t (*Read)(uint8_t address, uint8_t *buffer, uint8_t len); + int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); + int32_t (*Read)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); }; #ifdef PIOS_INCLUDE_SPI @@ -120,12 +122,12 @@ struct pios_hmc5x83_cfg { }; /* Public Functions */ -extern void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); -extern bool PIOS_HMC5x83_NewDataAvailable(void); -extern int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]); -extern uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]); -extern int32_t PIOS_HMC5x83_Test(void); -extern bool PIOS_HMC5x83_IRQHandler(); +extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); +extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler); +extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]); +extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]); +extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler); +extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler); #endif /* PIOS_HMC5x83_H */ From d856bbdb5015ed7d340a0f1779a6aa172bb69e0e Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:34:24 +0200 Subject: [PATCH 092/718] OP-1403 - Update targets --- flight/modules/Extensions/MagBaro/magbaro.c | 7 ++++--- flight/modules/Sensors/sensors.c | 11 ++++++++--- .../targets/boards/revolution/firmware/pios_board.c | 11 +++++++++-- flight/targets/boards/revoproto/firmware/pios_board.c | 10 ++++++++-- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index aa956c57e..73e2a37db 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -65,6 +65,7 @@ static int alt_ds_count = 0; #endif #if defined(PIOS_INCLUDE_HMC5X83) +pios_hmc5x83_dev_t mag_handle = 0; int32_t mag_test; static float mag_bias[3] = { 0, 0, 0 }; static float mag_scale[3] = { 1, 1, 1 }; @@ -151,7 +152,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); + mag_handle = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -199,9 +200,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters) #endif /* if defined(PIOS_INCLUDE_BMP085) */ #if defined(PIOS_INCLUDE_HMC5X83) - if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { + if (PIOS_HMC5x83_NewDataAvailable(mag_handle) || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { int16_t values[3]; - PIOS_HMC5x83_ReadMag(values); + PIOS_HMC5x83_ReadMag(mag_handle, values); float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], (float)values[0] * mag_scale[1] - mag_bias[1], -(float)values[2] * mag_scale[2] - mag_bias[2] }; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index c93dc64f8..b24e34317 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -82,6 +82,11 @@ static xTaskHandle sensorsTaskHandle; RevoCalibrationData cal; AccelGyroSettingsData agcal; +#ifdef PIOS_INCLUDE_HMC5X83 +#include +extern pios_hmc5x83_dev_t onboard_mag; +#endif + // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; @@ -201,7 +206,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } #if defined(PIOS_INCLUDE_HMC5X83) - mag_test = PIOS_HMC5x83_Test(); + mag_test = PIOS_HMC5x83_Test(onboard_mag); #else mag_test = 0; #endif @@ -411,9 +416,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; - PIOS_HMC5x83_ReadMag(values); + PIOS_HMC5x83_ReadMag(onboard_mag, values); float mags[3] = { (float)values[1] - mag_bias[0], (float)values[0] - mag_bias[1], -(float)values[2] - mag_bias[2] }; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index b97adb07a..b26f75a8c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -57,6 +57,7 @@ */ #if defined(PIOS_INCLUDE_ADC) + #include "pios_adc_priv.h" void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); @@ -93,8 +94,14 @@ void PIOS_ADC_DMC_irq_handler(void) #if defined(PIOS_INCLUDE_HMC5X83) #include "pios_hmc5x83.h" +pios_hmc5x83_dev_t onboard_mag = 0; + +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(onboard_mag); +} static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = PIOS_HMC5x83_IRQHandler, + .vector = pios_board_internal_mag_handler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -946,7 +953,7 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index de2c5982d..d748265bb 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -82,9 +82,15 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5X83) +pios_hmc5x83_dev_t onboard_mag = 0; + +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(onboard_mag); +} #include "pios_hmc5x83.h" static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = PIOS_HMC5x83_IRQHandler, + .vector = pios_board_internal_mag_handler, .line = EXTI_Line5, .pin = { .gpio = GPIOB, @@ -940,7 +946,7 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) From 016ba6940db8a9533d8c35e1e889d28656cbcc52 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:43:57 +0200 Subject: [PATCH 093/718] OP-1403 - drop attitude/revolution as it has been supersede by StateEstimation module --- flight/modules/Attitude/revolution/attitude.c | 1348 ----------------- .../Attitude/revolution/inc/attitude.h | 37 - 2 files changed, 1385 deletions(-) delete mode 100644 flight/modules/Attitude/revolution/attitude.c delete mode 100644 flight/modules/Attitude/revolution/inc/attitude.h diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c deleted file mode 100644 index 2ee8901e0..000000000 --- a/flight/modules/Attitude/revolution/attitude.c +++ /dev/null @@ -1,1348 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Copter Control Attitude Estimation - * @brief Acquires sensor data and computes attitude estimate - * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects - * @{ - * - * @file attitude.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Module to handle all comms to the AHRS on a periodic basis. - * - * @see The GNU Public License (GPL) Version 3 - * - ******************************************************************************/ -/* - * 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 - */ - -/** - * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeState - * - * This module computes an attitude estimate from the sensor data - * - * The module executes in its own thread. - * - * UAVObjects are automatically generated by the UAVObjectGenerator from - * the object definition XML file. - * - * Modules have no API, all communication to other modules is done through UAVObjects. - * However modules may use the API exposed by shared libraries. - * See the OpenPilot wiki for more details. - * http://www.openpilot.org/OpenPilot_Application_Architecture - * - */ - -#include -#include -#include "attitude.h" -#include "accelsensor.h" -#include "accelstate.h" -#include "airspeedsensor.h" -#include "airspeedstate.h" -#include "attitudestate.h" -#include "attitudesettings.h" -#include "barosensor.h" -#include "flightstatus.h" -#include "gpspositionsensor.h" -#include "gpsvelocitysensor.h" -#include "gyrostate.h" -#include "gyrosensor.h" -#include "homelocation.h" -#include "magsensor.h" -#include "magstate.h" -#include "positionstate.h" -#include "ekfconfiguration.h" -#include "ekfstatevariance.h" -#include "revocalibration.h" -#include "revosettings.h" -#include "velocitystate.h" -#include "taskinfo.h" - -#include "CoordinateConversions.h" - -// Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define FAILSAFE_TIMEOUT_MS 10 - -#define CALIBRATION_DELAY 4000 -#define CALIBRATION_DURATION 6000 -// low pass filter configuration to calculate offset -// of barometric altitude sensor -// reasoning: updates at: 10 Hz, tau= 300 s settle time -// exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f - -// simple IAS to TAS aproximation - 2% increase per 1000ft -// since we do not have flowing air temperature information -#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) - -// Private types - -// Private variables -static xTaskHandle attitudeTaskHandle; - -static xQueueHandle gyroQueue; -static xQueueHandle accelQueue; -static xQueueHandle magQueue; -static xQueueHandle airspeedQueue; -static xQueueHandle baroQueue; -static xQueueHandle gpsQueue; -static xQueueHandle gpsVelQueue; - -static AttitudeSettingsData attitudeSettings; -static HomeLocationData homeLocation; -static RevoCalibrationData revoCalibration; -static EKFConfigurationData ekfConfiguration; -static RevoSettingsData revoSettings; -static FlightStatusData flightStatus; -const uint32_t SENSOR_QUEUE_SIZE = 10; - -static bool volatile variance_error = true; -static bool volatile initialization_required = true; -static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running -static float rollPitchBiasRate = 0; - -// Accel filtering -static float accel_alpha = 0; -static bool accel_filter_enabled = false; -static float accels_filtered[3]; -static float grot_filtered[3]; - -// Private functions -static void AttitudeTask(void *parameters); - -static int32_t updateAttitudeComplementary(bool first_run); -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); -static void settingsUpdatedCb(UAVObjEvent *objEv); - -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED); - -static void magOffsetEstimation(MagSensorData *mag); - -// check for invalid values -static inline bool invalid(float data) -{ - if (isnan(data) || isinf(data)) { - return true; - } - return false; -} - -// check for invalid variance values -static inline bool invalid_var(float data) -{ - if (invalid(data)) { - return true; - } - if (data < 1e-15f) { // var should not be close to zero. And not negative either. - return true; - } - return false; -} - -/** - * API for sensor fusion algorithms: - * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) - * Stores all the queues the algorithm will pull data from - * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias) - * Update() -- queries queues and updates the attitude estiamte - */ - - -/** - * Initialise the module. Called before the start function - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeInitialize(void) -{ - GyroSensorInitialize(); - GyroStateInitialize(); - AccelSensorInitialize(); - AccelStateInitialize(); - MagSensorInitialize(); - MagStateInitialize(); - AirspeedSensorInitialize(); - AirspeedStateInitialize(); - BaroSensorInitialize(); - GPSPositionSensorInitialize(); - GPSVelocitySensorInitialize(); - AttitudeSettingsInitialize(); - AttitudeStateInitialize(); - PositionStateInitialize(); - VelocityStateInitialize(); - RevoSettingsInitialize(); - RevoCalibrationInitialize(); - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - FlightStatusInitialize(); - - // Initialize this here while we aren't setting the homelocation in GPS - HomeLocationInitialize(); - - // Initialize quaternion - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = 1.0f; - attitude.q2 = 0.0f; - attitude.q3 = 0.0f; - attitude.q4 = 0.0f; - AttitudeStateSet(&attitude); - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - RevoCalibrationConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); - EKFConfigurationConnectCallback(&settingsUpdatedCb); - FlightStatusConnectCallback(&settingsUpdatedCb); - - return 0; -} - -/** - * Start the task. Expects all objects to be initialized by this point. - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeStart(void) -{ - // Create the queues for the sensors - gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - - // Start main task - xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); -#endif - - GyroSensorConnectQueue(gyroQueue); - AccelSensorConnectQueue(accelQueue); - MagSensorConnectQueue(magQueue); - AirspeedSensorConnectQueue(airspeedQueue); - BaroSensorConnectQueue(baroQueue); - GPSPositionSensorConnectQueue(gpsQueue); - GPSVelocitySensorConnectQueue(gpsVelQueue); - - return 0; -} - -MODULE_INITCALL(AttitudeInitialize, AttitudeStart); - -/** - * Module thread, should not return. - */ -static void AttitudeTask(__attribute__((unused)) void *parameters) -{ - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - // Force settings update to make sure rotation loaded - settingsUpdatedCb(NULL); - - // Wait for all the sensors be to read - vTaskDelay(100); - - // Main task loop - TODO: make it run as delayed callback - while (1) { - int32_t ret_val = -1; - - bool first_run = false; - if (initialization_required) { - initialization_required = false; - first_run = true; - } - - // This function blocks on data queue - switch (running_algorithm) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - ret_val = updateAttitudeComplementary(first_run); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR: - ret_val = updateAttitudeINSGPS(first_run, true); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: - ret_val = updateAttitudeINSGPS(first_run, false); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - break; - } - - if (ret_val != 0) { - initialization_required = true; - } - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); -#endif - } -} - -static inline void apply_accel_filter(const float *raw, float *filtered) -{ - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); - } else { - filtered[0] = raw[0]; - filtered[1] = raw[1]; - filtered[2] = raw[2]; - } -} - -float accel_mag; -float qmag; -float attitudeDt; -float mag_err[3]; - -static int32_t updateAttitudeComplementary(bool first_run) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - GyroStateData gyroStateData; - AccelSensorData accelSensorData; - static int32_t timeval; - float dT; - static uint8_t init = 0; - static float gyro_bias[3] = { 0, 0, 0 }; - static bool magCalibrated = true; - static uint32_t initStartupTime = 0; - - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { - // When one of these is updated so should the other - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - AccelSensorGet(&accelSensorData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - // During initialization and - if (first_run) { -#if defined(PIOS_INCLUDE_HMC5X83) - // To initialize we need a valid mag reading - if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { - return -1; - } - MagSensorData magData; - MagSensorGet(&magData); -#else - MagSensorData magData; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; -#endif - float magBias[3]; - RevoCalibrationmag_biasArrayGet(magBias); - // don't trust Mag for initial orientation if it has not been calibrated - if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) { - magCalibrated = false; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; - } - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - init = 0; - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - timeval = PIOS_DELAY_GetRaw(); - - // wait calibration_delay only at powerup - if (xTaskGetTickCount() < 3000) { - initStartupTime = 0; - } else { - initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY; - } - - // Zero gyro bias - // This is really needed after updating calibration settings. - gyro_bias[0] = 0.0f; - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - return 0; - } - - if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) && - (xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) { - // For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup - // Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate. - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); - rollPitchBiasRate = 0.0f; - if (accel_alpha > 0.0f) { - accel_filter_enabled = true; - } - init = 1; - } - - GyroSensorGet(&gyroSensorData); - gyroStateData.x = gyroSensorData.x; - gyroStateData.y = gyroSensorData.y; - gyroStateData.z = gyroSensorData.z; - - // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); - - float q[4]; - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - float grot[3]; - float accel_err[3]; - - // Get the current attitude estimate - quat_copy(&attitudeState.q1, q); - - // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter((const float *)&accelSensorData.x, accels_filtered); - - // Rotate gravity to body frame and cross with accels - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - - apply_accel_filter(grot, grot_filtered); - - CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); - - // Account for accel magnitude - accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]; - accel_mag = sqrtf(accel_mag); - - float grot_mag; - if (accel_filter_enabled) { - grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); - } else { - grot_mag = 1.0f; - } - - // TODO! check grot_mag & accel vector magnitude values for correctness. - - accel_err[0] /= (accel_mag * grot_mag); - accel_err[1] /= (accel_mag * grot_mag); - accel_err[2] /= (accel_mag * grot_mag); - - - if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { - // Rotate gravity to body frame and cross with accels - float brot[3]; - float Rbe[3][3]; - MagSensorData mag; - - Quaternion2R(q, Rbe); - MagSensorGet(&mag); - -// TODO: separate filter! - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mag); - } - MagStateData mags; - mags.x = mag.x; - mags.y = mag.y; - mags.z = mag.z; - MagStateSet(&mags); - - // If the mag is producing bad data don't use it (normally bad calibration) - if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { - rot_mult(Rbe, homeLocation.Be, brot); - - float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); - mag.x /= mag_len; - mag.y /= mag_len; - mag.z /= mag_len; - - float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); - brot[0] /= bmag; - brot[1] /= bmag; - brot[2] /= bmag; - - // Only compute if neither vector is null - if (bmag < 1.0f || mag_len < 1.0f) { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } else { - CrossProduct((const float *)&mag.x, (const float *)brot, mag_err); - } - } - } else { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - // Correct rates based on integral coefficient - gyroStateData.x -= gyro_bias[0]; - gyroStateData.y -= gyro_bias[1]; - gyroStateData.z -= gyro_bias[2]; - - gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate; - gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate; - gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate; - - // save gyroscope state - GyroStateSet(&gyroStateData); - - // Correct rates based on proportional coefficient - gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT; - - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - if (q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - - // Renomalize - qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; - } - - quat_copy(q, &attitudeState.q1); - - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); - - AttitudeStateSet(&attitudeState); - - // Flush these queues for avoid errors - xQueueReceive(baroQueue, &ev, 0); - if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { - float NED[3]; - // Transform the GPS position into NED coordinates - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - getNED(&gpsPosition, NED); - - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = NED[0]; - positionState.East = NED[1]; - positionState.Down = NED[2]; - PositionStateSet(&positionState); - } - - if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { - // Transform the GPS position into NED coordinates - GPSVelocitySensorData gpsVelocity; - GPSVelocitySensorGet(&gpsVelocity); - - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = gpsVelocity.North; - velocityState.East = gpsVelocity.East; - velocityState.Down = gpsVelocity.Down; - VelocityStateSet(&velocityState); - } - - if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { - // Calculate true airspeed from indicated airspeed - AirspeedSensorData airspeedSensor; - AirspeedSensorGet(&airspeedSensor); - - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - PositionStateData positionState; - PositionStateGet(&positionState); - - if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - // we have airspeed available - airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; - AirspeedStateSet(&airspeed); - } - } - - if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - - return 0; -} - -#include "insgps.h" -int32_t ins_failed = 0; -extern struct NavStruct Nav; -int32_t init_stage = 0; - -/** - * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) - * @params[in] first_run This is the first run so trigger reinitialization - * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) - * @return 0 for success, -1 for failure - */ -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - AccelSensorData accelSensorData; - MagStateData magData; - AirspeedSensorData airspeedData; - BaroSensorData baroData; - GPSPositionSensorData gpsData; - GPSVelocitySensorData gpsVelData; - - static bool mag_updated = false; - static bool baro_updated; - static bool airspeed_updated; - static bool gps_updated; - static bool gps_vel_updated; - - static bool value_error = false; - - static float baroOffset = 0.0f; - - static uint32_t ins_last_time = 0; - static bool inited; - - float NED[3] = { 0.0f, 0.0f, 0.0f }; - float vel[3] = { 0.0f, 0.0f, 0.0f }; - float zeros[3] = { 0.0f, 0.0f, 0.0f }; - - // Perform the update - uint16_t sensors = 0; - float dT; - - // Wait until the gyro and accel object is updated, if a timeout then go to failsafe - if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || - (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - if (inited) { - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - } - - if (first_run) { - inited = false; - init_stage = 0; - - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - - ins_last_time = PIOS_DELAY_GetRaw(); - - return 0; - } - - mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); - baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - - // Check if we are running simulation - if (!GPSPositionSensorReadOnly()) { - gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_updated |= pdTRUE && outdoor_mode; - } - - if (!GPSVelocitySensorReadOnly()) { - gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_vel_updated |= pdTRUE && outdoor_mode; - } - - // Get most recent data - GyroSensorGet(&gyroSensorData); - AccelSensorGet(&accelSensorData); -// TODO: separate filter! - if (mag_updated) { - MagSensorData mags; - MagSensorGet(&mags); - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mags); - } - magData.x = mags.x; - magData.y = mags.y; - magData.z = mags.z; - MagStateSet(&magData); - } else { - MagStateGet(&magData); - } - - BaroSensorGet(&baroData); - AirspeedSensorGet(&airspeedData); - GPSPositionSensorGet(&gpsData); - GPSVelocitySensorGet(&gpsVelData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - - value_error = false; - // safety checks - if (invalid(gyroSensorData.x) || - invalid(gyroSensorData.y) || - invalid(gyroSensorData.z) || - invalid(accelSensorData.x) || - invalid(accelSensorData.y) || - invalid(accelSensorData.z)) { - // cannot run process update, raise error! - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return 0; - } - - if (invalid(magData.x) || - invalid(magData.y) || - invalid(magData.z)) { - // magnetometers can be ignored for a while - mag_updated = false; - value_error = true; - } - - // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily - // switching between indoor and outdoor mode with Set = false) - if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { - mag_updated = false; - value_error = true; - } - - if (invalid(baroData.Altitude)) { - baro_updated = false; - value_error = true; - } - - if (invalid(airspeedData.CalibratedAirspeed)) { - airspeed_updated = false; - value_error = true; - } - - if (invalid(gpsData.Altitude)) { - gps_updated = false; - value_error = true; - } - - if (invalid_var(ekfConfiguration.R.GPSPosNorth) || - invalid_var(ekfConfiguration.R.GPSPosEast) || - invalid_var(ekfConfiguration.R.GPSPosDown) || - invalid_var(ekfConfiguration.R.GPSVelNorth) || - invalid_var(ekfConfiguration.R.GPSVelEast) || - invalid_var(ekfConfiguration.R.GPSVelDown)) { - gps_updated = false; - value_error = true; - } - - if (invalid(gpsVelData.North) || - invalid(gpsVelData.East) || - invalid(gpsVelData.Down)) { - gps_vel_updated = false; - value_error = true; - } - - // Discard airspeed if sensor not connected - if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - airspeed_updated = false; - } - - // Have a minimum requirement for gps usage - if ((gpsData.Satellites < 7) || - (gpsData.PDOP > 4.0f) || - (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (homeLocation.Set != HOMELOCATION_SET_TRUE)) { - gps_updated = false; - gps_vel_updated = false; - } - - if (!inited) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (value_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (outdoor_mode && gpsData.Satellites < 7) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if (dT > 0.01f) { - dT = 0.01f; - } else if (dT <= 0.001f) { - dT = 0.001f; - } - - if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { - // Don't initialize until all sensors are read - if (init_stage == 0) { - // Reset the INS algorithm - INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R.MagX, - ekfConfiguration.R.MagY, - ekfConfiguration.R.MagZ } - ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX, - ekfConfiguration.Q.AccelY, - ekfConfiguration.Q.AccelZ } - ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX, - ekfConfiguration.Q.GyroY, - ekfConfiguration.Q.GyroZ } - ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX, - ekfConfiguration.Q.GyroDriftY, - ekfConfiguration.Q.GyroDriftZ } - ); - INSSetBaroVar(ekfConfiguration.R.BaroZ); - - // Initialize the gyro bias - float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; - INSSetGyroBias(gyro_bias); - - float pos[3] = { 0.0f, 0.0f, 0.0f }; - - if (outdoor_mode) { - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - - // Transform the GPS position into NED coordinates - getNED(&gpsPosition, pos); - - // Initialize barometric offset to current GPS NED coordinate - baroOffset = -pos[2] - baroData.Altitude; - } else { - // Initialize barometric offset to homelocation altitude - baroOffset = -baroData.Altitude; - pos[2] = -(baroData.Altitude + baroOffset); - } - - // xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - // MagSensorGet(&magData); - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; - INSSetState(pos, zeros, q, zeros, zeros); - - INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)); - } else { - // Run prediction a bit before any corrections - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - INSStatePrediction(gyros, &accelSensorData.x, dT); - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - } - - init_stage++; - if (init_stage > 10) { - inited = true; - } - - return 0; - } - - if (!inited) { - return 0; - } - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - - // Advance the state estimate - INSStatePrediction(gyros, &accelSensorData.x, dT); - - // Copy the attitude into the UAVO - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - - // Advance the covariance estimate - INSCovariancePrediction(dT); - - if (mag_updated) { - sensors |= MAG_SENSORS; - } - - if (baro_updated) { - sensors |= BARO_SENSOR; - } - - INSSetMagNorth(homeLocation.Be); - - if (gps_updated && outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth, - ekfConfiguration.R.GPSPosEast, - ekfConfiguration.R.GPSPosDown }, - (float[3]) { ekfConfiguration.R.GPSVelNorth, - ekfConfiguration.R.GPSVelEast, - ekfConfiguration.R.GPSVelDown } - ); - sensors |= POS_SENSORS; - - if (0) { // Old code to take horizontal velocity from GPS Position update - sensors |= HORIZ_SENSORS; - vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); - vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); - vel[2] = 0.0f; - } - // Transform the GPS position into NED coordinates - getNED(&gpsData, NED); - - // Track barometric altitude offset with a low pass filter - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-NED[2] - baroData.Altitude); - } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor } - ); - vel[0] = vel[1] = vel[2] = 0.0f; - NED[0] = NED[1] = 0.0f; - NED[2] = -(baroData.Altitude + baroOffset); - sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; - sensors |= POS_SENSORS | VERT_SENSORS; - } - - if (gps_vel_updated && outdoor_mode) { - sensors |= HORIZ_SENSORS | VERT_SENSORS; - vel[0] = gpsVelData.North; - vel[1] = gpsVelData.East; - vel[2] = gpsVelData.Down; - } - - // Copy the position into the UAVO - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = Nav.Pos[0]; - positionState.East = Nav.Pos[1]; - positionState.Down = Nav.Pos[2]; - PositionStateSet(&positionState); - - // airspeed correction needs current positionState - if (airspeed_updated) { - // we have airspeed available - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed; - - AirspeedStateSet(&airspeed); - - if (!gps_vel_updated && !gps_updated) { - // feed airspeed into EKF, treat wind as 1e2 variance - sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed } - ); - // rotate airspeed vector into NED frame - airspeed is measured in X axis only - float R[3][3]; - Quaternion2R(Nav.q, R); - float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f }; - rot_mult(R, vtas, vel); - } - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - if (sensors) { - INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors); - } - - // Copy the velocity into the UAVO - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = Nav.Vel[0]; - velocityState.East = Nav.Vel[1]; - velocityState.Down = Nav.Vel[2]; - VelocityStateSet(&velocityState); - - GyroStateData gyroState; - gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0])); - gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1])); - gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2])); - GyroStateSet(&gyroState); - - EKFStateVarianceData vardata; - EKFStateVarianceGet(&vardata); - INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); - EKFStateVarianceSet(&vardata); - - return 0; -} - -/** - * @brief Convert the GPS LLA position into NED coordinates - * @note this method uses a taylor expansion around the home coordinates - * to convert to NED which allows it to be done with all floating - * calculations - * @param[in] Current GPS coordinates - * @param[out] NED frame coordinates - * @returns 0 for success, -1 for failure - */ -float T[3]; -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED) -{ - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - - NED[0] = T[0] * dL[0]; - NED[1] = T[1] * dL[1]; - NED[2] = T[2] * dL[2]; - - return 0; -} - -static void settingsUpdatedCb(UAVObjEvent *ev) -{ - if (ev == NULL || ev->obj == FlightStatusHandle()) { - FlightStatusGet(&flightStatus); - } - if (ev == NULL || ev->obj == RevoCalibrationHandle()) { - RevoCalibrationGet(&revoCalibration); - } - // change of these settings require reinitialization of the EKF - // when an error flag has been risen, we also listen to flightStatus updates, - // since we are waiting for the system to get disarmed so we can reinitialize safely. - if (ev == NULL || - ev->obj == EKFConfigurationHandle() || - ev->obj == RevoSettingsHandle() || - (variance_error == true && ev->obj == FlightStatusHandle()) - ) { - bool error = false; - - EKFConfigurationGet(&ekfConfiguration); - int t; - for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) { - error = true; - } - } - - RevoSettingsGet(&revoSettings); - - // Reinitialization of the EKF is not desired during flight. - // It will be delayed until the board is disarmed by raising the error flag. - // We will not prevent the initial initialization though, since the board could be in always armed mode. - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) { - error = true; - } - - if (error) { - variance_error = true; - } else { - // trigger reinitialization - possibly with new algorithm - running_algorithm = revoSettings.FusionAlgorithm; - variance_error = false; - initialization_required = true; - } - } - if (ev == NULL || ev->obj == HomeLocationHandle()) { - HomeLocationGet(&homeLocation); - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; - - T[0] = alt + 6.378137E6f; - T[1] = cosf(lat) * (alt + 6.378137E6f); - T[2] = -1.0f; - - // TODO: convert positionState to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { - AttitudeSettingsGet(&attitudeSettings); - - // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. - const float fakeDt = 0.0015f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; - } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; - } - } -} - -/** - * Perform an update of the @ref MagBias based on - * Magmeter Offset Cancellation: Theory and Implementation, - * revisited William Premerlani, October 14, 2011 - */ -static void magOffsetEstimation(MagSensorData *mag) -{ - #if 0 - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; - - static float B2[3] = { 0, 0, 0 }; - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } - - float B1[3] = { mag->x, mag->y, mag->z }; - float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); - float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; - - MagBiasSet(&magBias); - - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } - #else // if 0 - static float magBias[3] = { 0 }; - - // Remove the current estimate of the bias - mag->x -= magBias[0]; - mag->y -= magBias[1]; - mag->z -= magBias[2]; - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = revoCalibration.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, Rot); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias[0] += delta[0]; - magBias[1] += delta[1]; - magBias[2] += delta[2]; - } - #endif // if 0 -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Attitude/revolution/inc/attitude.h b/flight/modules/Attitude/revolution/inc/attitude.h deleted file mode 100644 index 64c556a8e..000000000 --- a/flight/modules/Attitude/revolution/inc/attitude.h +++ /dev/null @@ -1,37 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Attitude Module - * @{ - * - * @file attitude.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Acquires sensor data and fuses it into attitude estimate for CC - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * 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 ATTITUDE_H -#define ATTITUDE_H - -#include "openpilot.h" - -int32_t AttitudeInitialize(void); - -#endif // ATTITUDE_H From a6502518beb95ce58919937770d03150a0dd4351 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 15:19:03 +0200 Subject: [PATCH 094/718] OP-1405 - Add support for 25qxxx series of flash memories --- flight/pios/inc/pios_flash_jedec_catalog.h | 29 +++++++++++++++++++++- flight/pios/inc/pios_flash_jedec_priv.h | 2 ++ 2 files changed, 30 insertions(+), 1 deletion(-) diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index 32e5a70c8..f35473538 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -71,7 +71,34 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .chip_erase = 0x60, .fast_read = 0x0B, .fast_read_dummy_bytes = 1, - } + }, + { // 25q512 + .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, + .expect_memorytype = 0xBA, + .expect_capacity = 0x20, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, + }, + { // 25q256 + .expect_manufacturer = JEDEC_MANUFACTURER_NUMORIX, + .expect_memorytype = 0xBA, + .expect_capacity = 0x19, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, + }, + { // 25q128 + .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, + .expect_memorytype = 0xBA, + .expect_capacity = 0x18, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, + }, }; const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog); diff --git a/flight/pios/inc/pios_flash_jedec_priv.h b/flight/pios/inc/pios_flash_jedec_priv.h index 6d1d3353d..765acf6e1 100644 --- a/flight/pios/inc/pios_flash_jedec_priv.h +++ b/flight/pios/inc/pios_flash_jedec_priv.h @@ -36,6 +36,8 @@ extern const struct pios_flash_driver pios_jedec_flash_driver; #define JEDEC_MANUFACTURER_ST 0x20 +#define JEDEC_MANUFACTURER_MICRON 0x20 +#define JEDEC_MANUFACTURER_NUMORIX 0x20 #define JEDEC_MANUFACTURER_MACRONIX 0xC2 #define JEDEC_MANUFACTURER_WINBOND 0xEF From 0976fb6e1bdb82aef7bdc14d0ffa39051b7915e2 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 18:30:47 +0200 Subject: [PATCH 095/718] OP-1404 - Add support for UBlox PVT sentence --- flight/modules/GPS/UBX.c | 44 +++++++++++++++++++++ flight/modules/GPS/inc/UBX.h | 75 +++++++++++++++++++++++++++++++----- 2 files changed, 109 insertions(+), 10 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 544e97372..e2a614d34 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -251,6 +251,47 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData * } } +void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPosition) +{ + GPSVelocitySensorData GpsVelocity; + + if (check_msgtracker(pvt->iTOW, ALL_RECEIVED)) { + if ((pvt->fixType == PVT_FIX_TYPE_3D) || + (pvt->fixType == PVT_FIX_TYPE_2D)) { + GpsVelocity.North = (float)pvt->velN / 100.0f; + GpsVelocity.East = (float)pvt->velE / 100.0f; + GpsVelocity.Down = (float)pvt->velD / 100.0f; + GPSVelocitySensorSet(&GpsVelocity); + + GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; + GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; + GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; + GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; + GpsPosition->Latitude = pvt->lat; + GpsPosition->Longitude = pvt->lon; + GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? + GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; + + GpsPosition->PDOP = (float)pvt->pDOP * 0.01f; + } else { + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; + } + + if (pvt->valid & PVT_VALID_VALIDTIME) { + // Time is valid, set GpsTime + GPSTimeData GpsTime; + + GpsTime.Year = pvt->year; + GpsTime.Month = pvt->month; + GpsTime.Day = pvt->day; + GpsTime.Hour = pvt->hour; + GpsTime.Minute = pvt->min; + GpsTime.Second = pvt->sec; + + GPSTimeSet(&GpsTime); + } + } +} #if !defined(PIOS_GPS_MINIMAL) void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) { @@ -324,6 +365,9 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi case UBX_ID_VELNED: parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); break; + case UBX_ID_PVT: + parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); + break; #if !defined(PIOS_GPS_MINIMAL) case UBX_ID_TIMEUTC: parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index fd1ee0fb1..dc76f32fd 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -35,22 +35,23 @@ #include "GPS.h" -#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters -#define UBX_SYNC2 0x62 +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 // From u-blox6 receiver protocol specification // Messages classes -#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_NAV 0x01 // Message IDs -#define UBX_ID_POSLLH 0x02 -#define UBX_ID_STATUS 0x03 -#define UBX_ID_DOP 0x04 -#define UBX_ID_SOL 0x06 -#define UBX_ID_VELNED 0x12 -#define UBX_ID_TIMEUTC 0x21 -#define UBX_ID_SVINFO 0x30 +#define UBX_ID_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 +#define UBX_ID_PVT 0x07 // private structures @@ -156,6 +157,59 @@ struct UBX_NAV_TIMEUTC { uint8_t valid; // Validity Flags }; +#define PVT_VALID_VALIDDATE 0x01 +#define PVT_VALID_VALIDTIME 0x02 +#define PVT_VALID_FULLYRESOLVED 0x04 + +#define PVT_FIX_TYPE_NO_FIX 0 +#define PVT_FIX_TYPE_DEAD_RECKON 0x01 // Dead Reckoning only +#define PVT_FIX_TYPE_2D 0x02 // 2D-Fix +#define PVT_FIX_TYPE_3D 0x03 // 3D-Fix +#define PVT_FIX_TYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined +#define PVT_FIX_TYPE_TIME_ONLY 0x05 // Time only fix + +#define PVT_FLAGS_GNNSFIX_OK (1 << 0) +#define PVT_FLAGS_DIFFSOLN (1 << 1) +#define PVT_FLAGS_PSMSTATE_ENABLED (1 << 2) +#define PVT_FLAGS_PSMSTATE_ACQUISITION (2 << 2) +#define PVT_FLAGS_PSMSTATE_TRACKING (3 << 2) +#define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2) +#define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2) + +// PVT Navigation Position Velocity Time Solution +struct UBX_NAV_PVT { + uint32_t iTOW; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; + uint32_t tAcc; + int32_t nano; + uint8_t fixType; + uint8_t flags; + uint8_t reserved1; + uint8_t numSV; + int32_t lon; + int32_t lat; + int32_t height; + int32_t hMSL; + uint32_t hAcc; + uint32_t vAcc; + int32_t velN; + int32_t velE; + int32_t velD; + int32_t gSpeed; + int32_t heading; + uint32_t sAcc; + uint32_t headingAcc; + uint32_t pDOP; + uint16_t reserved2; + uint32_t reserved3; +}; + // Space Vehicle (SV) Information // Single SV information block @@ -198,6 +252,7 @@ typedef union { struct UBX_NAV_DOP nav_dop; struct UBX_NAV_SOL nav_sol; struct UBX_NAV_VELNED nav_velned; + struct UBX_NAV_PVT nav_pvt; #if !defined(PIOS_GPS_MINIMAL) struct UBX_NAV_TIMEUTC nav_timeutc; struct UBX_NAV_SVINFO nav_svinfo; From 1dd05c24ccebe6869b2f125e773a542cf3ee4a1d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:35:19 +0200 Subject: [PATCH 096/718] OP-1404 - Fix compilation with PIOS_GPS_MINIMAL defined --- flight/modules/GPS/UBX.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index e2a614d34..232d56de2 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -276,7 +276,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi } else { GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; } - +#if !defined(PIOS_GPS_MINIMAL) if (pvt->valid & PVT_VALID_VALIDTIME) { // Time is valid, set GpsTime GPSTimeData GpsTime; @@ -290,6 +290,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi GPSTimeSet(&GpsTime); } +#endif } } #if !defined(PIOS_GPS_MINIMAL) From c68dd3c00e4045600031eceb11fd9302310476d4 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Aug 2014 15:04:40 +0200 Subject: [PATCH 097/718] OP-1404 - PVT will provide SOL+VELNED+POSLLH+Time. DOP will provide all dop values --- flight/modules/GPS/UBX.c | 61 ++++++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 31 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 232d56de2..c114d1c5c 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -255,43 +255,42 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi { GPSVelocitySensorData GpsVelocity; - if (check_msgtracker(pvt->iTOW, ALL_RECEIVED)) { - if ((pvt->fixType == PVT_FIX_TYPE_3D) || - (pvt->fixType == PVT_FIX_TYPE_2D)) { - GpsVelocity.North = (float)pvt->velN / 100.0f; - GpsVelocity.East = (float)pvt->velE / 100.0f; - GpsVelocity.Down = (float)pvt->velD / 100.0f; - GPSVelocitySensorSet(&GpsVelocity); + msgtracker.currentTOW = pvt->iTOW; + msgtracker.msg_received = (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED); - GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; - GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; - GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; - GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; - GpsPosition->Latitude = pvt->lat; - GpsPosition->Longitude = pvt->lon; - GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? - GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; + GpsVelocity.North = (float)pvt->velN / 100.0f; + GpsVelocity.East = (float)pvt->velE / 100.0f; + GpsVelocity.Down = (float)pvt->velD / 100.0f; + GPSVelocitySensorSet(&GpsVelocity); - GpsPosition->PDOP = (float)pvt->pDOP * 0.01f; - } else { - GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; - } + GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; + GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; + GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; + GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; + GpsPosition->Latitude = pvt->lat; + GpsPosition->Longitude = pvt->lon; + GpsPosition->Satellites = pvt->numSV; + if(pvt->flags & PVT_FLAGS_GNNSFIX_OK) { + GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? + GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; + } else { + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; + } #if !defined(PIOS_GPS_MINIMAL) - if (pvt->valid & PVT_VALID_VALIDTIME) { - // Time is valid, set GpsTime - GPSTimeData GpsTime; + if (pvt->valid & PVT_VALID_VALIDTIME) { + // Time is valid, set GpsTime + GPSTimeData GpsTime; - GpsTime.Year = pvt->year; - GpsTime.Month = pvt->month; - GpsTime.Day = pvt->day; - GpsTime.Hour = pvt->hour; - GpsTime.Minute = pvt->min; - GpsTime.Second = pvt->sec; + GpsTime.Year = pvt->year; + GpsTime.Month = pvt->month; + GpsTime.Day = pvt->day; + GpsTime.Hour = pvt->hour; + GpsTime.Minute = pvt->min; + GpsTime.Second = pvt->sec; - GPSTimeSet(&GpsTime); - } + GPSTimeSet(&GpsTime); + } #endif - } } #if !defined(PIOS_GPS_MINIMAL) void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) From 6f4b1d5c03570d66556496e6bc1492c076d1e664 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 3 Aug 2014 15:27:30 +0200 Subject: [PATCH 098/718] OP-1404 uncrustify --- flight/modules/GPS/UBX.c | 58 ++++++++++++++++++------------------ flight/modules/GPS/inc/UBX.h | 22 +++++++------- 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index c114d1c5c..df69fcf9e 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -255,41 +255,41 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi { GPSVelocitySensorData GpsVelocity; - msgtracker.currentTOW = pvt->iTOW; + msgtracker.currentTOW = pvt->iTOW; msgtracker.msg_received = (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED); - GpsVelocity.North = (float)pvt->velN / 100.0f; - GpsVelocity.East = (float)pvt->velE / 100.0f; - GpsVelocity.Down = (float)pvt->velD / 100.0f; - GPSVelocitySensorSet(&GpsVelocity); + GpsVelocity.North = (float)pvt->velN / 100.0f; + GpsVelocity.East = (float)pvt->velE / 100.0f; + GpsVelocity.Down = (float)pvt->velD / 100.0f; + GPSVelocitySensorSet(&GpsVelocity); - GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; - GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; - GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; - GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; - GpsPosition->Latitude = pvt->lat; - GpsPosition->Longitude = pvt->lon; - GpsPosition->Satellites = pvt->numSV; - if(pvt->flags & PVT_FLAGS_GNNSFIX_OK) { - GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? - GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; - } else { - GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; - } + GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; + GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; + GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; + GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; + GpsPosition->Latitude = pvt->lat; + GpsPosition->Longitude = pvt->lon; + GpsPosition->Satellites = pvt->numSV; + if (pvt->flags & PVT_FLAGS_GNNSFIX_OK) { + GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? + GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; + } else { + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; + } #if !defined(PIOS_GPS_MINIMAL) - if (pvt->valid & PVT_VALID_VALIDTIME) { - // Time is valid, set GpsTime - GPSTimeData GpsTime; + if (pvt->valid & PVT_VALID_VALIDTIME) { + // Time is valid, set GpsTime + GPSTimeData GpsTime; - GpsTime.Year = pvt->year; - GpsTime.Month = pvt->month; - GpsTime.Day = pvt->day; - GpsTime.Hour = pvt->hour; - GpsTime.Minute = pvt->min; - GpsTime.Second = pvt->sec; + GpsTime.Year = pvt->year; + GpsTime.Month = pvt->month; + GpsTime.Day = pvt->day; + GpsTime.Hour = pvt->hour; + GpsTime.Minute = pvt->min; + GpsTime.Second = pvt->sec; - GPSTimeSet(&GpsTime); - } + GPSTimeSet(&GpsTime); + } #endif } #if !defined(PIOS_GPS_MINIMAL) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index dc76f32fd..996dfd8d4 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -35,23 +35,23 @@ #include "GPS.h" -#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters -#define UBX_SYNC2 0x62 +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 // From u-blox6 receiver protocol specification // Messages classes -#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_NAV 0x01 // Message IDs -#define UBX_ID_POSLLH 0x02 -#define UBX_ID_STATUS 0x03 -#define UBX_ID_DOP 0x04 -#define UBX_ID_SOL 0x06 -#define UBX_ID_VELNED 0x12 -#define UBX_ID_TIMEUTC 0x21 -#define UBX_ID_SVINFO 0x30 -#define UBX_ID_PVT 0x07 +#define UBX_ID_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 +#define UBX_ID_PVT 0x07 // private structures From d3569927a0b4340359bb273bc59422027e282148 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 4 Aug 2014 11:04:35 +0200 Subject: [PATCH 099/718] OP-1404 fix unit scaling for pvt, use check_msgtracker, --- flight/modules/GPS/UBX.c | 18 ++++++++---------- flight/modules/GPS/inc/UBX.h | 2 +- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index df69fcf9e..220792164 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -32,9 +32,8 @@ #include "pios.h" #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) - -#include "UBX.h" -#include "GPS.h" +#include "inc\UBX.h" +#include "inc\GPS.h" // parse incoming character stream for messages in UBX binary format @@ -255,22 +254,21 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi { GPSVelocitySensorData GpsVelocity; - msgtracker.currentTOW = pvt->iTOW; - msgtracker.msg_received = (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED); + check_msgtracker(pvt->iTOW, (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED)); - GpsVelocity.North = (float)pvt->velN / 100.0f; - GpsVelocity.East = (float)pvt->velE / 100.0f; - GpsVelocity.Down = (float)pvt->velD / 100.0f; + GpsVelocity.North = (float)pvt->velN * 0.001f; + GpsVelocity.East = (float)pvt->velE * 0.001f; + GpsVelocity.Down = (float)pvt->velD * 0.001f; GPSVelocitySensorSet(&GpsVelocity); - GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f; + GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.001f; GpsPosition->Heading = (float)pvt->heading * 1.0e-5f; GpsPosition->Altitude = (float)pvt->hMSL * 0.001f; GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f; GpsPosition->Latitude = pvt->lat; GpsPosition->Longitude = pvt->lon; GpsPosition->Satellites = pvt->numSV; - if (pvt->flags & PVT_FLAGS_GNNSFIX_OK) { + if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) { GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; } else { diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 996dfd8d4..923eb6b42 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -168,7 +168,7 @@ struct UBX_NAV_TIMEUTC { #define PVT_FIX_TYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined #define PVT_FIX_TYPE_TIME_ONLY 0x05 // Time only fix -#define PVT_FLAGS_GNNSFIX_OK (1 << 0) +#define PVT_FLAGS_GNSSFIX_OK (1 << 0) #define PVT_FLAGS_DIFFSOLN (1 << 1) #define PVT_FLAGS_PSMSTATE_ENABLED (1 << 2) #define PVT_FLAGS_PSMSTATE_ACQUISITION (2 << 2) From 240eb8e573ec9ec1fed39c0df9eb4ca0dc32face Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 4 Aug 2014 17:56:28 +0200 Subject: [PATCH 100/718] OP-1404 - Prevent mixing pvt and other sentence solution data. use PDOP from pvt --- flight/modules/GPS/UBX.c | 71 ++++++++++++++++++++++++++---------- flight/modules/GPS/inc/UBX.h | 2 +- 2 files changed, 53 insertions(+), 20 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 220792164..c0b925936 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -35,6 +35,8 @@ #include "inc\UBX.h" #include "inc\GPS.h" +// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC +#define UBX_PVT_TIMEOUT (1000) // parse incoming character stream for messages in UBX binary format int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) @@ -254,7 +256,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi { GPSVelocitySensorData GpsVelocity; - check_msgtracker(pvt->iTOW, (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED)); + check_msgtracker(pvt->iTOW, (ALL_RECEIVED)); GpsVelocity.North = (float)pvt->velN * 0.001f; GpsVelocity.East = (float)pvt->velE * 0.001f; @@ -268,6 +270,7 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi GpsPosition->Latitude = pvt->lat; GpsPosition->Longitude = pvt->lon; GpsPosition->Satellites = pvt->numSV; + GpsPosition->PDOP = pvt->pDOP * 0.01f; if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) { GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ? GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; @@ -347,33 +350,63 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) { uint32_t id = 0; + static uint32_t lastPvtTime; + static bool ubxInitialized = false; + + if (!ubxInitialized) { + // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0. + GpsPosition->HDOP = 99.99f; + GpsPosition->PDOP = 99.99f; + GpsPosition->VDOP = 99.99f; + ubxInitialized = true; + } + // is it using PVT? + bool usePvt = (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000); switch (ubx->header.class) { case UBX_CLASS_NAV: - switch (ubx->header.id) { - case UBX_ID_POSLLH: - parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); - break; - case UBX_ID_DOP: - parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); - break; - case UBX_ID_SOL: - parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); - break; - case UBX_ID_VELNED: - parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); - break; + if (!usePvt) { + switch (ubx->header.id) { + case UBX_ID_POSLLH: + parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); + break; + case UBX_ID_DOP: + parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); + break; + case UBX_ID_SOL: + parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); + break; + case UBX_ID_VELNED: + parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); + break; + case UBX_ID_PVT: + parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); + lastPvtTime = PIOS_DELAY_GetuS(); + break; +#if !defined(PIOS_GPS_MINIMAL) + case UBX_ID_TIMEUTC: + parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); + break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); + break; +#endif + } + } else { + switch (ubx->header.id) { + case UBX_ID_DOP: + parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); + break; + } case UBX_ID_PVT: parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); - break; -#if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_TIMEUTC: - parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); + lastPvtTime = PIOS_DELAY_GetuS(); break; case UBX_ID_SVINFO: parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); break; -#endif + default: + break; } break; } diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 923eb6b42..575684240 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -208,7 +208,7 @@ struct UBX_NAV_PVT { uint32_t pDOP; uint16_t reserved2; uint32_t reserved3; -}; +} __attribute__((packed)); // Space Vehicle (SV) Information From ea63def1c8de2faac92486bfc9ed246a4862292c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 4 Aug 2014 18:17:01 +0200 Subject: [PATCH 101/718] OP-1404 fix compilation with PIOS_GPS_MINIMAL, fix some copy/pastem madness --- flight/modules/GPS/UBX.c | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index c0b925936..1b699c923 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -397,16 +397,19 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi case UBX_ID_DOP: parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); break; + + case UBX_ID_PVT: + parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); + lastPvtTime = PIOS_DELAY_GetuS(); + break; +#if !defined(PIOS_GPS_MINIMAL) + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); + break; +#endif + default: + break; } - case UBX_ID_PVT: - parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); - lastPvtTime = PIOS_DELAY_GetuS(); - break; - case UBX_ID_SVINFO: - parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); - break; - default: - break; } break; } From 8821754b4a005f826ea031f62124da2cec8c4fcb Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 5 Aug 2014 12:49:35 +0200 Subject: [PATCH 102/718] OP-1425 Small_typo_KpKi : Fix typo + french translation / updated translation for error messages --- .../translations/openpilotgcs_fr.ts | 65 ++++++++++++++----- .../src/plugins/config/stabilization.ui | 12 ++-- 2 files changed, 55 insertions(+), 22 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index 38cd9e7f3..c60686c9d 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -7934,14 +7934,12 @@ Useful if you have accidentally changed some settings. <html><head/><body><p>Ceci ajuste le niveau de stabilité que votre véhicule aura en vol incliné (ex. vol en avançant) en mode Attitude. Ajouter une valeur d'intégrale en mode Attitude lorsque une intégrale est présente en mode Rate n'est pas recommandé.</p></body></html> - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in Rate mode.</p></body></html> - <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KP) qui est utilisée en mode Rate.</p></body></html> + <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KP) qui est utilisée en mode Rate.</p></body></html> - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in AttitudeMode.</p></body></html> - <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KP) qui est utilisée en mode Attitude.</p></body></html> + <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KP) qui est utilisée en mode Attitude.</p></body></html> @@ -8234,6 +8232,16 @@ Useful if you have accidentally changed some settings. <html><head/><body><p>Motor response time to go from min thrust to max thrust. It allows thrust anticipation on entering/exiting inverted mode</p></body></html> <html><head/><body><p>Temps de réponse du moteur pour passer de la poussée minimum à la poussée maximum. Cela permet une anticipation lors de l'entrée/sortie du mode inversé.</p></body></html> + + + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in Rate mode.</p></body></html> + <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KI) qui est utilisée en mode Rate.</p></body></html> + + + + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in AttitudeMode.</p></body></html> + <html><head/><body><p>Ceci limite la valeur maximale de l'intégrale (KI) qui est utilisée en mode Attitude.</p></body></html> + TxPIDWidget @@ -8946,7 +8954,7 @@ les données en cache Diagramme de Connexion - + Save File Enregistrer Fichier @@ -9297,17 +9305,19 @@ p, li { white-space: pre-wrap; } Hexacoptère - + Hexacopter Coax (Y6) Hexacoptère Coax (Y6) - + + Hexacopter X Hexacoptère X + Hexacopter H Hexacoptère H @@ -9392,7 +9402,7 @@ p, li { white-space: pre-wrap; } - + @@ -10256,11 +10266,29 @@ p, li { white-space: pre-wrap; } ConfigMultiRotorWidget - - + + + + + + + + + + Configuration OK Configuration OK + + + <font color='red'>ERROR: Assign a Yaw channel</font> + <font color='red'>ERREUR : Veuillez affecter le canal de Yaw</font> + + + + <font color='red'>ERROR: Assign all %1 motor channels</font> + <font color='red'>ERREUR : Veuillez affecter tous les %1 canaux moteurs</font> + ConfigCCHWWidget @@ -10857,7 +10885,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende. - + @@ -10865,7 +10893,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.Inconnu - + Vehicle type: Type de véhicule : @@ -10904,6 +10932,11 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.Hexacopter Coax (Y6) Hexacoptère Coax (Y6) + + + Hexacopter H + Hexacoptère H + Hexacopter X @@ -11023,7 +11056,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.Écriture paramètres matériels - + Writing actuator settings Écriture paramètres actionneurs @@ -11069,7 +11102,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.Écriture contrôles manuels par défaut - + Preparing mixer settings Préparation des paramètres de mixage @@ -13938,7 +13971,7 @@ et même conduire au crash. A utiliser avec prudence. ConfigVehicleTypeWidget - + Multirotor Multirotor @@ -13963,7 +13996,7 @@ et même conduire au crash. A utiliser avec prudence. Personnalisé - + http://wiki.openpilot.org/x/44Cf Lien Wiki FR http://wiki.openpilot.org/x/IIBqAQ diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 0840ed0c6..f2e5042dd 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -16641,7 +16641,7 @@ border-radius: 5; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in Rate mode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in Rate mode.</p></body></html> @@ -16694,7 +16694,7 @@ border-radius: 5; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in Rate mode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in Rate mode.</p></body></html> @@ -17851,7 +17851,7 @@ font:bold; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in AttitudeMode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in AttitudeMode.</p></body></html> @@ -17932,7 +17932,7 @@ font:bold; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in AttitudeMode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in AttitudeMode.</p></body></html> @@ -18042,7 +18042,7 @@ font:bold; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in AttitudeMode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in AttitudeMode.</p></body></html> @@ -18657,7 +18657,7 @@ font:bold; Qt::StrongFocus - <html><head/><body><p>This sets the maximum value of the integral (KP) that is used in Rate mode.</p></body></html> + <html><head/><body><p>This sets the maximum value of the integral (KI) that is used in Rate mode.</p></body></html> From 23d23689845e8e2296e500b7bb959e47aec1366e Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 5 Aug 2014 16:50:38 +0200 Subject: [PATCH 103/718] OP-1404 Fix nav-pvt.pdop data type --- flight/modules/GPS/inc/UBX.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 575684240..f32a267bc 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -205,7 +205,7 @@ struct UBX_NAV_PVT { int32_t heading; uint32_t sAcc; uint32_t headingAcc; - uint32_t pDOP; + uint16_t pDOP; uint16_t reserved2; uint32_t reserved3; } __attribute__((packed)); From d04d630273c7d635d956b9f234c2870ccfa0a9b6 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 5 Aug 2014 17:25:28 +0200 Subject: [PATCH 104/718] OP-1404 fix include file path --- flight/modules/GPS/UBX.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 1b699c923..63628421e 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -32,8 +32,8 @@ #include "pios.h" #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) -#include "inc\UBX.h" -#include "inc\GPS.h" +#include "inc/UBX.h" +#include "inc/GPS.h" // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC #define UBX_PVT_TIMEOUT (1000) From 4754e62a8ef45cca29e92463218b58e209da03f7 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Tue, 5 Aug 2014 19:31:26 +0200 Subject: [PATCH 105/718] OP-1423 Use the TxPIDSettings::PIDsOptions enum instead of string comparisons --- .../src/plugins/config/configtxpidwidget.cpp | 164 +++++++++++------- .../src/plugins/config/configtxpidwidget.h | 4 +- 2 files changed, 99 insertions(+), 69 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index 5447f3ec1..3facff7fa 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -55,9 +55,9 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings())); connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings())); - connect(m_txpid->PID1, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); - connect(m_txpid->PID2, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); - connect(m_txpid->PID3, SIGNAL(currentTextChanged(QString)), this, SLOT(updateSpinBoxProperties(const QString &))); + connect(m_txpid->PID1, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int))); + connect(m_txpid->PID2, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int))); + connect(m_txpid->PID3, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int))); addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true); @@ -97,75 +97,105 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget() } template -static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, const QString & pidOption) +static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, int pidOption) { - if (pidOption == "Disabled") { + switch (pidOption) { + case TxPIDSettings::PIDS_DISABLED: return 0.0f; - } else if (pidOption == "Roll Rate.Kp") { - return bank->getRollRatePID_Kp(); - } else if (pidOption == "Pitch Rate.Kp") { - return bank->getPitchRatePID_Kp(); - } else if (pidOption == "Roll+Pitch Rate.Kp") { - return bank->getRollRatePID_Kp(); - } else if (pidOption == "Yaw Rate.Kp") { - return bank->getYawRatePID_Kp(); - } else if (pidOption == "Roll Rate.Ki") { - return bank->getRollRatePID_Ki(); - } else if (pidOption == "Pitch Rate.Ki") { - return bank->getPitchRatePID_Ki(); - } else if (pidOption == "Roll+Pitch Rate.Ki") { - return bank->getRollRatePID_Ki(); - } else if (pidOption == "Yaw Rate.Ki") { - return bank->getYawRatePID_Ki(); - } else if (pidOption == "Roll Rate.Kd") { - return bank->getRollRatePID_Kd(); - } else if (pidOption == "Pitch Rate.Kd") { - return bank->getPitchRatePID_Kd(); - } else if (pidOption == "Roll+Pitch Rate.Kd") { - return bank->getRollRatePID_Kd(); - } else if (pidOption == "Yaw Rate.Kd") { - return bank->getYawRatePID_Kd(); - } else if (pidOption == "Roll Rate.ILimit") { - return bank->getRollRatePID_ILimit(); - } else if (pidOption == "Pitch Rate.ILimit") { - return bank->getPitchRatePID_ILimit(); - } else if (pidOption == "Roll+Pitch Rate.ILimit") { - return bank->getRollRatePID_ILimit(); - } else if (pidOption == "Yaw Rate.ILimit") { - return bank->getYawRatePID_ILimit(); - } else if (pidOption == "Roll Attitude.Kp") { - return bank->getRollPI_Kp(); - } else if (pidOption == "Pitch Attitude.Kp") { - return bank->getPitchPI_Kp(); - } else if (pidOption == "Roll+Pitch Attitude.Kp") { - return bank->getRollPI_Kp(); - } else if (pidOption == "Yaw Attitude.Kp") { - return bank->getYawPI_Kp(); - } else if (pidOption == "Roll Attitude.Ki") { - return bank->getRollPI_Ki(); - } else if (pidOption == "Pitch Attitude.Ki") { - return bank->getPitchPI_Ki(); - } else if (pidOption == "Roll+Pitch Attitude.Ki") { - return bank->getRollPI_Ki(); - } else if (pidOption == "Yaw Attitude.Ki") { - return bank->getYawPI_Ki(); - } else if (pidOption == "Roll Attitude.ILimit") { - return bank->getRollPI_ILimit(); - } else if (pidOption == "Pitch Attitude.ILimit") { - return bank->getPitchPI_ILimit(); - } else if (pidOption == "Roll+Pitch Attitude.ILimit") { - return bank->getRollPI_ILimit(); - } else if (pidOption == "Yaw Attitude.ILimit") { - return bank->getYawPI_ILimit(); - } - qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption; - return 0.0f; + case TxPIDSettings::PIDS_ROLLRATEKP: + return bank->getRollRatePID_Kp(); + + case TxPIDSettings::PIDS_PITCHRATEKP: + return bank->getPitchRatePID_Kp(); + + case TxPIDSettings::PIDS_ROLLPITCHRATEKP: + return bank->getRollRatePID_Kp(); + + case TxPIDSettings::PIDS_YAWRATEKP: + return bank->getYawRatePID_Kp(); + + case TxPIDSettings::PIDS_ROLLRATEKI: + return bank->getRollRatePID_Ki(); + + case TxPIDSettings::PIDS_PITCHRATEKI: + return bank->getPitchRatePID_Ki(); + + case TxPIDSettings::PIDS_ROLLPITCHRATEKI: + return bank->getRollRatePID_Ki(); + + case TxPIDSettings::PIDS_YAWRATEKI: + return bank->getYawRatePID_Ki(); + + case TxPIDSettings::PIDS_ROLLRATEKD: + return bank->getRollRatePID_Kd(); + + case TxPIDSettings::PIDS_PITCHRATEKD: + return bank->getPitchRatePID_Kd(); + + case TxPIDSettings::PIDS_ROLLPITCHRATEKD: + return bank->getRollRatePID_Kd(); + + case TxPIDSettings::PIDS_YAWRATEKD: + return bank->getYawRatePID_Kd(); + + case TxPIDSettings::PIDS_ROLLRATEILIMIT: + return bank->getRollRatePID_ILimit(); + + case TxPIDSettings::PIDS_PITCHRATEILIMIT: + return bank->getPitchRatePID_ILimit(); + + case TxPIDSettings::PIDS_ROLLPITCHRATEILIMIT: + return bank->getRollRatePID_ILimit(); + + case TxPIDSettings::PIDS_YAWRATEILIMIT: + return bank->getYawRatePID_ILimit(); + + case TxPIDSettings::PIDS_ROLLATTITUDEKP: + return bank->getRollPI_Kp(); + + case TxPIDSettings::PIDS_PITCHATTITUDEKP: + return bank->getPitchPI_Kp(); + + case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP: + return bank->getRollPI_Kp(); + + case TxPIDSettings::PIDS_YAWATTITUDEKP: + return bank->getYawPI_Kp(); + + case TxPIDSettings::PIDS_ROLLATTITUDEKI: + return bank->getRollPI_Ki(); + + case TxPIDSettings::PIDS_PITCHATTITUDEKI: + return bank->getPitchPI_Ki(); + + case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI: + return bank->getRollPI_Ki(); + + case TxPIDSettings::PIDS_YAWATTITUDEKI: + return bank->getYawPI_Ki(); + + case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT: + return bank->getRollPI_ILimit(); + + case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT: + return bank->getPitchPI_ILimit(); + + case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT: + return bank->getRollPI_ILimit(); + + case TxPIDSettings::PIDS_YAWATTITUDEILIMIT: + return bank->getYawPI_ILimit(); + + default: + qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption; + return 0.0f; + } } -float ConfigTxPIDWidget::getDefaultValueForPidOption(const QString & pidOption) +float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption) { - if (pidOption == "GyroTau") { + if (pidOption == TxPIDSettings::PIDS_GYROTAU) { StabilizationSettings *stab = qobject_cast(getObject(QString("StabilizationSettings"))); return stab->getGyroTau(); } @@ -187,7 +217,7 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(const QString & pidOption) } } -void ConfigTxPIDWidget::updateSpinBoxProperties(const QString & selectedPidOption) +void ConfigTxPIDWidget::updateSpinBoxProperties(int selectedPidOption) { QObject *PIDx = sender(); diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h index 34a50c413..afb148119 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.h @@ -40,8 +40,8 @@ private: Ui_TxPIDWidget *m_txpid; private slots: - void updateSpinBoxProperties(const QString & selectedPidOption); - float getDefaultValueForPidOption(const QString & pidOption); + void updateSpinBoxProperties(int selectedPidOption); + float getDefaultValueForPidOption(int pidOption); void refreshValues(); void applySettings(); void saveSettings(); From a18009e8758a744d125a8a86f81940f141dc84d8 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 6 Aug 2014 12:55:07 +0200 Subject: [PATCH 106/718] OP-1429 Change datatype for elevation and azimuth respectively to int8 and int16. --- flight/modules/GPS/UBX.c | 8 ++++---- shared/uavobjectdefinition/gpssatellites.xml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 63628421e..c4b80f900 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -325,8 +325,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) svdata.SatsInView = 0; for (chan = 0; chan < svinfo->numCh; chan++) { if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { - svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; - svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; + svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev; svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; svdata.SatsInView++; @@ -334,8 +334,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) } // fill remaining slots (if any) for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { - svdata.Azimuth[chan] = (float)0.0f; - svdata.Elevation[chan] = (float)0.0f; + svdata.Azimuth[chan] = 0; + svdata.Elevation[chan] = 0; svdata.PRN[chan] = 0; svdata.SNR[chan] = 0; } diff --git a/shared/uavobjectdefinition/gpssatellites.xml b/shared/uavobjectdefinition/gpssatellites.xml index 8d3ecb8e3..9c5b87453 100644 --- a/shared/uavobjectdefinition/gpssatellites.xml +++ b/shared/uavobjectdefinition/gpssatellites.xml @@ -3,8 +3,8 @@ Contains information about the GPS satellites in view from @ref GPSModule. - - + + From dfb21482f467f426d46d5b719f8ae89d24579548 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gast=C3=A9=20Olivier?= Date: Fri, 8 Aug 2014 13:14:39 +0200 Subject: [PATCH 107/718] DSM protocol improvement added item in DSM protocol in GCS --- flight/pios/inc/pios_dsm_priv.h | 3 +- flight/pios/stm32f4xx/pios_dsm.c | 41 +++++-------------- .../coptercontrol/firmware/pios_board.c | 10 +++-- .../boards/revolution/firmware/pios_board.c | 20 ++++++--- .../plugins/setupwizard/connectiondiagram.cpp | 3 +- .../plugins/setupwizard/pages/inputpage.cpp | 18 +++++--- .../src/plugins/setupwizard/setupwizard.cpp | 11 +++-- .../vehicleconfigurationhelper.cpp | 17 +++++--- .../setupwizard/vehicleconfigurationsource.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 14 +++---- 10 files changed, 76 insertions(+), 63 deletions(-) diff --git a/flight/pios/inc/pios_dsm_priv.h b/flight/pios/inc/pios_dsm_priv.h index 1cdf88f75..d823b4f3d 100644 --- a/flight/pios/inc/pios_dsm_priv.h +++ b/flight/pios/inc/pios_dsm_priv.h @@ -111,7 +111,8 @@ /* DSM protocol variations */ enum pios_dsm_proto { - PIOS_DSM_PROTO_DSM2, + PIOS_DSM_PROTO_DSM210BIT, + PIOS_DSM_PROTO_DSM211BIT, PIOS_DSM_PROTO_DSMX10BIT, PIOS_DSM_PROTO_DSMX11BIT, }; diff --git a/flight/pios/stm32f4xx/pios_dsm.c b/flight/pios/stm32f4xx/pios_dsm.c index b6d8f0d85..70b32483d 100644 --- a/flight/pios/stm32f4xx/pios_dsm.c +++ b/flight/pios/stm32f4xx/pios_dsm.c @@ -181,7 +181,7 @@ static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev) static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) { struct pios_dsm_state *state = &(dsm_dev->state); - uint8_t resolution; + uint8_t resolution=10; #ifdef DSM_LOST_FRAME_COUNTER /* increment the lost frame counter */ @@ -191,35 +191,16 @@ static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev) #endif /* check the frame type assuming master satellite stream */ - uint8_t type = state->received_data[1]; - switch (type) { - case 0x01: - case 0x02: - case 0x12: - /* DSM2, DSMJ stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) { - /* DSM2/DSMJ resolution is known from the header */ - resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - case 0xA2: - case 0xB2: - /* DSMX stream */ - if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) { - resolution = 10; - } else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) { - resolution = 11; - } else { - /* DSMX resolution should explicitly be selected */ - goto stream_error; - } - break; - default: - /* unknown yet data stream */ - goto stream_error; + + switch(dsm_dev->proto) { + case PIOS_DSM_PROTO_DSM210BIT: + case PIOS_DSM_PROTO_DSMX10BIT: + resolution = 10; + break; + case PIOS_DSM_PROTO_DSM211BIT: + case PIOS_DSM_PROTO_DSMX11BIT: + resolution = 11; + break; } /* unroll channels */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 2cb1734ff..35029acd6 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -477,15 +477,19 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_DSM2: + case HWSETTINGS_CC_MAINPORT_DSM210BIT: + case HWSETTINGS_CC_MAINPORT_DSM211BIT: case HWSETTINGS_CC_MAINPORT_DSMX10BIT: case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_CC_MAINPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_CC_MAINPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_CC_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 4901cc432..ebbedfb19 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -624,14 +624,18 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_MAINPORT_DSM2: + case HWSETTINGS_RM_MAINPORT_DSM210BIT: + case HWSETTINGS_RM_MAINPORT_DSM211BIT: case HWSETTINGS_RM_MAINPORT_DSMX10BIT: case HWSETTINGS_RM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_mainport) { - case HWSETTINGS_RM_MAINPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_RM_MAINPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_RM_MAINPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_RM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; @@ -693,14 +697,18 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; - case HWSETTINGS_RM_FLEXIPORT_DSM2: + case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: + case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_flexiport) { - case HWSETTINGS_RM_FLEXIPORT_DSM2: - proto = PIOS_DSM_PROTO_DSM2; + case HWSETTINGS_RM_FLEXIPORT_DSM210BIT: + proto = PIOS_DSM_PROTO_DSM210BIT; + break; + case HWSETTINGS_RM_FLEXIPORT_DSM211BIT: + proto = PIOS_DSM_PROTO_DSM211BIT; break; case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp index 22b85bdd3..8b1f56d92 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/connectiondiagram.cpp @@ -139,7 +139,8 @@ void ConnectionDiagram::setupGraphicsScene() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2_11: elementsToShow << "satellite"; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp index 5515a31dd..5c9ec16b7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.cpp @@ -54,7 +54,7 @@ bool InputPage::validatePage() } else if (ui->sbusButton->isChecked()) { getWizard()->setInputType(SetupWizard::INPUT_SBUS); } else if (ui->spectrumButton->isChecked()) { - getWizard()->setInputType(SetupWizard::INPUT_DSM2); + getWizard()->setInputType(SetupWizard::INPUT_DSM2_11); } else { getWizard()->setInputType(SetupWizard::INPUT_PWM); } @@ -85,9 +85,13 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.CC_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: // TODO: Handle all of the DSM types ?? Which is most common? - return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM2; + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; + + case VehicleConfigurationSource::INPUT_DSM2_11: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.CC_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; default: return true; } @@ -105,9 +109,13 @@ bool InputPage::restartNeeded(VehicleConfigurationSource::INPUT_TYPE selectedTyp case VehicleConfigurationSource::INPUT_SBUS: return data.RM_MainPort != HwSettings::CC_MAINPORT_SBUS; - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: // TODO: Handle all of the DSM types ?? Which is most common? - return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM2; + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM210BIT; + + case VehicleConfigurationSource::INPUT_DSM2_11: + // TODO: Handle all of the DSM types ?? Which is most common? + return data.RM_MainPort != HwSettings::CC_MAINPORT_DSM211BIT; default: return true; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 1657ca526..cc2cfd253 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -252,14 +252,17 @@ QString SetupWizard::getSummaryText() case INPUT_SBUS: summary.append(tr("Futaba S.Bus")); break; - case INPUT_DSM2: - summary.append(tr("Spektrum satellite (DSM2)")); + case INPUT_DSM2_10: + summary.append(tr("Spektrum satellite (DSM2 10bits)")); + break; + case INPUT_DSM2_11: + summary.append(tr("Spektrum satellite (DSM2 11bits)")); break; case INPUT_DSMX10: - summary.append(tr("Spektrum satellite (DSMX10BIT)")); + summary.append(tr("Spektrum satellite (DSMX 10bits)")); break; case INPUT_DSMX11: - summary.append(tr("Spektrum satellite (DSMX11BIT)")); + summary.append(tr("Spektrum satellite (DSMX 11bits)")); break; default: summary.append(tr("Unknown")); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 119d58120..a318d2fd4 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -139,8 +139,11 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2: - data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM2; + case VehicleConfigurationSource::INPUT_DSM2_10: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM210BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2_11: + data.CC_FlexiPort = HwSettings::CC_FLEXIPORT_DSM211BIT; break; default: break; @@ -172,8 +175,11 @@ void VehicleConfigurationHelper::applyHardwareConfiguration() case VehicleConfigurationSource::INPUT_DSMX11: data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSMX11BIT; break; - case VehicleConfigurationSource::INPUT_DSM2: - data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM2; + case VehicleConfigurationSource::INPUT_DSM2_10: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM210BIT; + break; + case VehicleConfigurationSource::INPUT_DSM2_11: + data.RM_FlexiPort = HwSettings::RM_FLEXIPORT_DSM211BIT; break; default: break; @@ -486,7 +492,8 @@ void VehicleConfigurationHelper::applyManualControlDefaults() break; case VehicleConfigurationSource::INPUT_DSMX10: case VehicleConfigurationSource::INPUT_DSMX11: - case VehicleConfigurationSource::INPUT_DSM2: + case VehicleConfigurationSource::INPUT_DSM2_10: + case VehicleConfigurationSource::INPUT_DSM2_11: channelType = ManualControlSettings::CHANNELGROUPS_DSMMAINPORT; break; default: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 797283656..3c0399421 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -62,7 +62,7 @@ public: 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 }; + enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2_10, INPUT_DSM2_11, INPUT_UNKNOWN }; enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; enum RADIO_SETTING { RADIO_TELEMETRY, RADIO_DISABLED }; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..556397340 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,19 +2,19 @@ Selection of optional hardware configurations. - - + + - - - + + + - - + + From 717de2964885b4e1e4d391fe09ba8c6f04e89b50 Mon Sep 17 00:00:00 2001 From: m_thread Date: Fri, 8 Aug 2014 13:48:36 +0200 Subject: [PATCH 108/718] OP-1400 Upgraded make and copydata files to use Qt 5.3.1 Opensource. --- ground/openpilotgcs/copydata.pro | 4 --- make/tools.mk | 52 ++++++++++++++++---------------- 2 files changed, 26 insertions(+), 30 deletions(-) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 933b77d76..b07a9eada 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,10 +30,6 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ - libicui18n.so.51 \ - libicuuc.so.51 \ - libicudata.so.51 \ - libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() for(lib, QT_LIBS) { diff --git a/make/tools.mk b/make/tools.mk index cddb119a2..28cfbbfc2 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -9,8 +9,8 @@ # Ready to use: # arm_sdk_install # qt_sdk_install -# mingw_install (Windows only - NOT USED for Qt-5.1.x) -# python_install (Windows only - NOT USED for Qt-5.1.x) +# mingw_install (Windows only - NOT USED for Qt-5.3.x) +# python_install (Windows only - NOT USED for Qt-5.3.x) # nsis_install (Windows only) # sdl_install (Windows only) # openssl_install (Windows only) @@ -61,14 +61,14 @@ ifeq ($(UNAME), Linux) ifeq ($(ARCH), x86_64) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2 ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5 - QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x64-5.2.1.run - QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x64-5.2.1.run.md5 + QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x64-5.3.1.run + QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x64-5.3.1.run.md5 QT_SDK_ARCH := gcc_64 else ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2 ARM_SDK_MD5_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5 - QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x86-5.2.1.run - QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x86-5.2.1.run.md5 + QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x86-5.3.1.run + QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x86-5.3.1.run.md5 QT_SDK_ARCH := gcc endif UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz @@ -76,14 +76,14 @@ ifeq ($(UNAME), Linux) else ifeq ($(UNAME), Darwin) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2 ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2/+md5 - QT_SDK_URL := "Please install native Qt 5.1.x SDK using package manager" + QT_SDK_URL := "Please install native Qt 5.3.x SDK using package manager" UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz DOXYGEN_URL := http://wiki.openpilot.org/download/attachments/18612236/doxygen-1.8.3.1.src.tar.gz else ifeq ($(UNAME), Windows) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip/+md5 - QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe - QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe.md5 + QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe + QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe.md5 QT_SDK_ARCH := mingw48_32 NSIS_URL := http://wiki.openpilot.org/download/attachments/18612236/nsis-2.46-unicode.tar.bz2 SDL_URL := http://wiki.openpilot.org/download/attachments/18612236/SDL-devel-1.2.15-mingw32.tar.gz @@ -97,7 +97,7 @@ GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0 # Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1 -QT_SDK_DIR := $(TOOLS_DIR)/qt-5.2.1 +QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1 MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32 PYTHON_DIR := $(QT_SDK_DIR)/Tools/mingw48_32/opt/bin NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode @@ -356,10 +356,10 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1mingw48_essentials.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1x32-4.8.0-release-posix-dwarf-rev2-runtime.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1icu_51_1_mingw_builds_4_8_0_posix_dwarf_32.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.addons/5.2.1mingw48_addons.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1mingw48_essentials.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1x32-4.8.0-release-posix-dwarf-rev2-runtime.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1icu_51_1_mingw_builds_4_8_0_posix_dwarf_32.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.addons/5.3.1mingw48_addons.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw48/4.8.0-1-1x32-4.8.0-release-posix-dwarf-rev2.7z" | grep -v Extracting # Run patcher @$(ECHO) @@ -420,15 +420,15 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) $(DL_DIR)/$(5) --dump-binary-data -o $(1) # Extract packages under tool directory $(V1) $(MKDIR) -p $$(call toprel, $(dir $(2))) - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1$(6)_qt5_essentials.7z" | grep -v Extracting - $(V1) if [ -f "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_64.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_64.7z" | grep -v Extracting; fi - $(V1) if [ -f "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_32.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_32.7z" | grep -v Extracting; fi -# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_path_patcher.sh.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).addons/5.2.1$(6)_qt5_addons.7z" | grep -v Extracting -# go to OpenPilot/tools/5.1.1/gcc_64 and call patcher.sh + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_essentials.7z" | grep -v Extracting + $(V1) if [ -f "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_64.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_64.7z" | grep -v Extracting; fi + $(V1) if [ -f "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_32.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_32.7z" | grep -v Extracting; fi +# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6).essentials/5.3.1icu_path_patcher.sh.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_addons.7z" | grep -v Extracting +# go to OpenPilot/tools/5.3.1/gcc_64 and call patcher.sh @$(ECHO) @$(ECHO) "Running patcher in" $$(call toprel, $(QT_SDK_PREFIX)) $(V1) $(CD) $(QT_SDK_PREFIX) @@ -502,7 +502,7 @@ endef ifeq ($(UNAME), Windows) -QT_SDK_PREFIX := $(QT_SDK_DIR)/5.2.1/$(QT_SDK_ARCH) +QT_SDK_PREFIX := $(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH) # This additional configuration step should not be necessary # but it is needed as a workaround to https://bugreports.qt-project.org/browse/QTBUG-33254 @@ -517,7 +517,7 @@ QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD else ifeq ($(UNAME), Linux) -QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.2.1/$(QT_SDK_ARCH)" +QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)" QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD $(eval $(call LINUX_QT_INSTALL_TEMPLATE,$(QT_BUILD_DIR),$(QT_SDK_DIR),$(QT_SDK_URL),$(QT_SDK_MD5_URL),$(notdir $(QT_SDK_URL)),$(QT_SDK_ARCH))) @@ -528,7 +528,7 @@ QT_SDK_PREFIX := $(QT_SDK_DIR) .PHONY: qt_sdk_install qt_sdk_install: @$(ECHO) $(MSG_NOTICE) -------------------------------------------------------- - @$(ECHO) $(MSG_NOTICE) Please install native Qt 5.2.x SDK using package manager + @$(ECHO) $(MSG_NOTICE) Please install native Qt 5.3.x SDK using package manager @$(ECHO) $(MSG_NOTICE) -------------------------------------------------------- .PHONY: qt_sdk_clean From c90bf0f8e0e5f6a7ccb02bfcb3d710f6c73d74ce Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 9 Aug 2014 04:26:16 +0200 Subject: [PATCH 109/718] OP-1432 Reverse_all_motors_gui_fix : Fixes all Hexacopters error when Gcs load "Reverse all motors" parameter. --- .../config/cfg_vehicletypes/configmultirotorwidget.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index 9f9a6334c..8216ca7d3 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -425,7 +425,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) // get motor 2 value for Yaw and Roll channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - setYawMixLevel(-qRound(value / 1.27)); + setYawMixLevel(qRound(value / 1.27)); // change channels channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; @@ -454,7 +454,7 @@ void ConfigMultiRotorWidget::refreshWidgetsValues(QString frameType) // get motor 2 value for Yaw and Roll channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_YAW); - setYawMixLevel(-qRound(value / 1.27)); + setYawMixLevel(qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); @@ -480,9 +480,9 @@ 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)); + setYawMixLevel(qRound(value / 1.27)); channel = m_aircraft->multiMotorChannelBox2->currentIndex() - 1; value = getMixerVectorValue(mixer, channel, VehicleConfig::MIXERVECTOR_ROLL); From 2932045e3dbac3df4d9573734d6bc2809e37bc58 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 9 Aug 2014 12:04:25 +0200 Subject: [PATCH 110/718] OP-1404 Fix NMEA data parsing for GSV --- flight/modules/GPS/NMEA.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index e8df40a7f..7aac3b5c2 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -687,8 +687,8 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD // Get sat info gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); - gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); - gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); + gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]); + gsv_partial.Azimuth[sat_index] = atoi(param[parIdx++]); gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); #ifdef NMEA_DEBUG_GSV DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); From 1158c70a4691f4df683661df8b2376913525fcea Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 9 Aug 2014 12:49:21 +0200 Subject: [PATCH 111/718] OP-1404 - minor cleanup, removed some duplicated code --- flight/modules/GPS/UBX.c | 46 ++++++++++++++++------------------------ 1 file changed, 18 insertions(+), 28 deletions(-) diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index c4b80f900..86910ef0d 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -350,8 +350,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo) uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) { uint32_t id = 0; - static uint32_t lastPvtTime; - static bool ubxInitialized = false; + static uint32_t lastPvtTime = 0; + static bool ubxInitialized = false; if (!ubxInitialized) { // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0. @@ -361,58 +361,48 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi ubxInitialized = true; } // is it using PVT? - bool usePvt = (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000); + bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000); switch (ubx->header.class) { case UBX_CLASS_NAV: if (!usePvt) { + // Set of messages to be decoded when not using pvt switch (ubx->header.id) { case UBX_ID_POSLLH: parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition); break; - case UBX_ID_DOP: - parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); - break; case UBX_ID_SOL: parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); break; case UBX_ID_VELNED: parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition); break; - case UBX_ID_PVT: - parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); - lastPvtTime = PIOS_DELAY_GetuS(); - break; #if !defined(PIOS_GPS_MINIMAL) case UBX_ID_TIMEUTC: parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc); break; - case UBX_ID_SVINFO: - parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); - break; #endif } - } else { - switch (ubx->header.id) { - case UBX_ID_DOP: - parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); - break; + } + // messages used always + switch (ubx->header.id) { + case UBX_ID_DOP: + parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); + break; - case UBX_ID_PVT: - parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); - lastPvtTime = PIOS_DELAY_GetuS(); - break; + case UBX_ID_PVT: + parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition); + lastPvtTime = PIOS_DELAY_GetuS(); + break; #if !defined(PIOS_GPS_MINIMAL) - case UBX_ID_SVINFO: - parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); - break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); + break; #endif - default: - break; - } } break; } + if (msgtracker.msg_received == ALL_RECEIVED) { GPSPositionSensorSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; From ca1923f76fabec18f1272f4f8a7395831e974c37 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 12:50:38 +0200 Subject: [PATCH 112/718] OP-1156: Added new (stub template) pathFollower Module as well as builkd environment changes to compile it instead of old pathfollowers --- flight/libraries/sanitycheck.c | 1 - flight/modules/PathFollower/pathfollower.c | 138 ++++++++++++++++++ .../boards/discoveryf4bare/firmware/Makefile | 3 +- .../boards/revolution/firmware/Makefile | 3 +- .../boards/revoproto/firmware/Makefile | 3 +- .../targets/boards/simposix/firmware/Makefile | 3 +- shared/uavobjectdefinition/callbackinfo.xml | 3 + shared/uavobjectdefinition/hwsettings.xml | 2 +- shared/uavobjectdefinition/taskinfo.xml | 3 - 9 files changed, 146 insertions(+), 13 deletions(-) create mode 100644 flight/modules/PathFollower/pathfollower.c diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 365e1af7b..0902cbce7 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -149,7 +149,6 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); - ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)); ADDSEVERITY(navCapableFusion); break; default: diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c new file mode 100644 index 000000000..3a719ffd7 --- /dev/null +++ b/flight/modules/PathFollower/pathfollower.c @@ -0,0 +1,138 @@ +/** + ****************************************************************************** + * + * @file pathfollower.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint + * and sets @ref AttitudeDesired. It only does this when the FlightMode field + * of @ref ManualControlCommand is Auto. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 + */ + +/** + * Input object: PathDesired + * Input object: PositionState + * Input object: ManualControlCommand + * Output object: StabilizationDesired + * + * This module acts as "autopilot" - it controls the setpoints of stabilization + * based on current flight situation and desired flight path (PathDesired) as + * directed by flightmode selection or pathplanner + * This is a periodic delayed callback module + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include + +#include + +#include +#include +#include + + +#include +#include +#include +#include + + +// Private constants + +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +#define PF_IDLE_UPDATE_RATE_MS 100 + +#define STACK_SIZE_BYTES 2048 +// Private types + +// Private variables +static DelayedCallbackInfo *pathFollowerCBInfo; + +static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; + +// Private functions +static void pathFollowerTask(void); +static void SettingsUpdatedCb(UAVObjEvent *ev); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerStart() +{ + // Start main task + SettingsUpdatedCb(NULL); + PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); + + return 0; +} + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t PathFollowerInitialize() +{ + // initialize objects + FixedWingPathFollowerSettingsInitialize(); + VtolPathFollowerSettingsInitialize(); + FlightStatusInitialize(); + PathStatusInitialize(); + + // Create object queue + + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + + return 0; +} +MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); + + +/** + * Module thread, should not return. + */ +static void pathFollowerTask(void) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); + return; + } + + // do pathfollower things here + + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); +} + +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + // read in settings (TODO) +} diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..bc8e72959 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -35,7 +35,6 @@ USE_DSP_LIB ?= NO #MODULES += Airspeed #MODULES += AltitudeHold #MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Actuator MODULES += GPS @@ -45,7 +44,7 @@ MODULES += Battery MODULES += FirmwareIAP #MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..52b5f8a64 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -46,7 +45,7 @@ MODULES += Battery MODULES += FirmwareIAP MODULES += Radio MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 39d2d7f3b..9933124a3 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -35,7 +35,6 @@ MODULES += Altitude/revolution MODULES += Airspeed #MODULES += AltitudeHold # now integrated in Stabilization MODULES += Stabilization -MODULES += VtolPathFollower MODULES += ManualControl MODULES += Receiver MODULES += Actuator @@ -45,7 +44,7 @@ MODULES += CameraStab MODULES += Battery MODULES += FirmwareIAP MODULES += PathPlanner -MODULES += FixedWingPathFollower +MODULES += PathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 2c89a81ef..d8af95e4a 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -33,8 +33,7 @@ DEBUG ?= YES # List of modules to include MODULES = ManualControl Stabilization GPS MODULES += PathPlanner -MODULES += FixedWingPathFollower -MODULES += VtolPathFollower +MODULES += PathFollower MODULES += CameraStab MODULES += Telemetry MODULES += Logging diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 8624867c4..598e4988a 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -8,6 +8,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -20,6 +21,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl @@ -36,6 +38,7 @@ AltitudeHold Stabilization0 Stabilization1 + PathFollower PathPlanner0 PathPlanner1 ManualControl diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f005a31ab..c41187009 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -22,7 +22,7 @@ - + diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index a5fb7ad4b..f8018ab53 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -19,7 +19,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -52,7 +51,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx @@ -89,7 +87,6 @@ Airspeed MagBaro - PathFollower FlightPlan TelemetryTx From 8944419b0fcb361a22816af3a494f2c2cdeca0d7 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 19:41:40 +0200 Subject: [PATCH 113/718] OP-1156 refactored pathfollower - new unified module as delayed callback - compiles but untested --- flight/libraries/math/mathmisc.h | 27 + flight/libraries/paths.c | 168 +-- flight/libraries/plans.c | 1 + flight/modules/PathFollower/pathfollower.c | 1084 ++++++++++++++++- .../VtolPathFollower/vtolpathfollower.c | 23 +- .../vtolpathfollowersettings.xml | 25 +- 6 files changed, 1219 insertions(+), 109 deletions(-) diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index b4c8b75b8..42213e504 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -52,4 +52,31 @@ static inline float boundf(float val, float boundary1, float boundary2) return val; } +static inline float squaref(float x) +{ + return x * x; +} + +static inline float vector_lengthf(float *vector, const uint8_t dim) +{ + float length = 0.0f; + + for (int t = 0; t < dim; t++) { + length += squaref(vector[t]); + } + return sqrtf(length); +} + +static inline void vector_normalizef(float *vector, const uint8_t dim) +{ + float length = vector_lengthf(vector, dim); + + if (length <= 0.0f || isnan(length)) { + return; + } + for (int t = 0; t < dim; t++) { + vector[t] /= length; + } +} + #endif /* MATHMISC_H */ diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 19176c443..6c80d0a9d 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -53,7 +53,7 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc case PATHDESIRED_MODE_FLYVECTOR: return path_vector(start_point, end_point, cur_point, status, true); - break; + break; case PATHDESIRED_MODE_DRIVEVECTOR: return path_vector(start_point, end_point, cur_point, status, false); @@ -98,23 +98,23 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to end - diff[0] = end_point[0] - cur_point[0]; - diff[1] = end_point[1] - cur_point[1]; - diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; + diff[0] = end_point[0] - cur_point[0]; + diff[1] = end_point[1] - cur_point[1]; + diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; - dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]); - dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]); + dist_diff = vector_lengthf(diff, 3); + dist_path = vector_lengthf(path, 3); if (dist_diff < 1e-6f) { status->fractional_progress = 1; status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; - status->path_direction[2] = 1; + status->path_direction[0] = status->path_direction[1] = 0; + status->path_direction[2] = 0; return; } @@ -147,23 +147,23 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, float track_point[3]; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + path[0] = end_point[0] - start_point[0]; + path[1] = end_point[1] - start_point[1]; + path[2] = mode3D ? end_point[2] - start_point[2] : 0; // Current progress location relative to start - diff[0] = cur_point[0] - start_point[0]; - diff[1] = cur_point[1] - start_point[1]; - diff[2] = mode3D ? cur_point[2] - start_point[2]: 0; + diff[0] = cur_point[0] - start_point[0]; + diff[1] = cur_point[1] - start_point[1]; + diff[2] = mode3D ? cur_point[2] - start_point[2] : 0; - dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; - dist_path = sqrtf(path[0] * path[0] + path[1] * path[1] + path[2] * path[2]); + dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; + dist_path = vector_lengthf(path, 3); if (dist_path > 1e-6f) { // Compute direction to travel & progress - status->path_direction[0] = path[0] / dist_path; - status->path_direction[1] = path[1] / dist_path; - status->path_direction[2] = path[2] / dist_path; + status->path_direction[0] = path[0] / dist_path; + status->path_direction[1] = path[1] / dist_path; + status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { // Fly towards the endpoint to prevent flying away, @@ -182,9 +182,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, status->correction_direction[1] = track_point[1] - cur_point[1]; status->correction_direction[2] = track_point[2] - cur_point[2]; - status->error = sqrt(status->correction_direction[0] * status->correction_direction[0] + - status->correction_direction[1] * status->correction_direction[1] + - status->correction_direction[2] * status->correction_direction[2]); + status->error = vector_lengthf(status->correction_direction, 3); // Normalize correction_direction but avoid division by zero if (status->error > 1e-6f) { @@ -194,7 +192,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, } else { status->correction_direction[0] = 0; status->correction_direction[1] = 0; - status->correction_direction[2] = 1; + status->correction_direction[2] = 0; } } @@ -207,7 +205,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point, */ static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) { - float radius_north, radius_east, diff_north, diff_east; + float radius_north, radius_east, diff_north, diff_east, diff_down; float radius, cradius; float normal[2]; float progress; @@ -220,70 +218,72 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, // Current location relative to center diff_north = cur_point[0] - end_point[0]; diff_east = cur_point[1] - end_point[1]; + diff_down = cur_point[2] - end_point[2]; - radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2)); - cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2)); + radius = sqrtf(squaref(radius_north) + squaref(radius_east)); + cradius = sqrtf(squaref(diff_north) + squaref(diff_east)); - if (cradius < 1e-6f) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->error = radius; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->correction_direction[2] = 0; - status->path_direction[0] = 1; - status->path_direction[1] = 0; - status->path_direction[2] = 0; - return; - } - - if (clockwise) { - // Compute the normal to the radius clockwise - normal[0] = -diff_east / cradius; - normal[1] = diff_north / cradius; - } else { - // Compute the normal to the radius counter clockwise - normal[0] = diff_east / cradius; - normal[1] = -diff_north / cradius; - } - - // normalize progress to 0..1 - a_diff = atan2f(diff_north, diff_east); - a_radius = atan2f(radius_north, radius_east); - - if (a_diff < 0) { - a_diff += 2.0f * M_PI_F; - } - if (a_radius < 0) { - a_radius += 2.0f * M_PI_F; - } - - progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); - - if (progress < 0) { - progress += 1.0f; - } else if (progress >= 1.0f) { - progress -= 1.0f; - } - - if (clockwise) { - progress = 1 - progress; - } - - status->fractional_progress = progress; + // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply) + status->path_direction[2] = 0.0f; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; - // Compute direction to correct error - status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius; - status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius; - status->correction_direction[2] = 0; + if (cradius < 1e-6f) { + // cradius is zero, just fly somewhere and make sure correction is still a normal + status->fractional_progress = 1; + status->correction_direction[0] = 0; + status->correction_direction[1] = 1; + status->path_direction[0] = 1; + status->path_direction[1] = 0; + } else { + if (clockwise) { + // Compute the normal to the radius clockwise + normal[0] = -diff_east / cradius; + normal[1] = diff_north / cradius; + } else { + // Compute the normal to the radius counter clockwise + normal[0] = diff_east / cradius; + normal[1] = -diff_north / cradius; + } - // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; - status->path_direction[2] = 0; + // normalize progress to 0..1 + a_diff = atan2f(diff_north, diff_east); + a_radius = atan2f(radius_north, radius_east); + + if (a_diff < 0) { + a_diff += 2.0f * M_PI_F; + } + if (a_radius < 0) { + a_radius += 2.0f * M_PI_F; + } + + progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F); + + if (progress < 0.0f) { + progress += 1.0f; + } else if (progress >= 1.0f) { + progress -= 1.0f; + } + + if (clockwise) { + progress = 1.0f - progress; + } + + status->fractional_progress = progress; + + // Compute direction to travel + status->path_direction[0] = normal[0]; + status->path_direction[1] = normal[1]; + + // Compute direction to correct error + status->correction_direction[0] = status->error * diff_north / cradius; + status->correction_direction[1] = status->error * diff_east / cradius; + } + + status->correction_direction[2] = -diff_down; + + vector_normalizef(status->correction_direction, 3); status->error = fabs(status->error); } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 10df7e52c..4df2a9289 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -132,6 +132,7 @@ static PiOSDeltatimeConfig landdT; void plan_setup_land() { float descendspeed; + plan_setup_positionHold(); FlightModeSettingsLandingVelocityGet(&descendspeed); diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 3a719ffd7..93efab3c9 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -51,12 +51,29 @@ #include #include #include +#include +#include +#include +#include #include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Private constants @@ -67,16 +84,53 @@ #define PF_IDLE_UPDATE_RATE_MS 100 #define STACK_SIZE_BYTES 2048 + +#define DEADBAND_HIGH 0.10f +#define DEADBAND_LOW -0.10f // Private types +struct Integrals { + float vel[3]; + float course; + float speed; + float power; + float airspeed; + float poiRadius; + bool vtolEmergencyFallback; +}; + + // Private variables static DelayedCallbackInfo *pathFollowerCBInfo; - static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; +static struct Integrals i; +static PathStatusData pathStatus; +static PathDesiredData pathDesired; +static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; +static VtolPathFollowerSettingsData vtolPathFollowerSettings; + +// correct speed by measured airspeed +static float indicatedAirspeedStateBias = 0.0f; + // Private functions static void pathFollowerTask(void); +static void resetIntegrals(); static void SettingsUpdatedCb(UAVObjEvent *ev); +static uint8_t updateAutoPilotByFrameType(); +static uint8_t updateAutoPilotFixedWing(); +static uint8_t updateAutoPilotVtol(); +static float updateTailInBearing(); +static float updateCourseBearing(); +static float updatePOIBearing(); +static void processPOI(); +static void updatePathVelocity(float kFF, float kH, float kV, bool limited); +static uint8_t updateFixedDesiredAttitude(); +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); +static void updateFixedAttitude(); +static void updateVtolDesiredAttitudeEmergencyFallback(); +static void airspeedStateUpdatedCb(UAVObjEvent *ev); +static bool correctCourse(float *C, float *V, float *F, float s); /** * Initialise the module, called on startup @@ -85,6 +139,7 @@ static void SettingsUpdatedCb(UAVObjEvent *ev); int32_t PathFollowerStart() { // Start main task + PathStatusGet(&pathStatus); SettingsUpdatedCb(NULL); PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo); @@ -99,15 +154,32 @@ int32_t PathFollowerInitialize() { // initialize objects FixedWingPathFollowerSettingsInitialize(); + FixedWingPathFollowerStatusInitialize(); VtolPathFollowerSettingsInitialize(); FlightStatusInitialize(); PathStatusInitialize(); + PathDesiredInitialize(); + PositionStateInitialize(); + VelocityStateInitialize(); + VelocityDesiredInitialize(); + StabilizationDesiredInitialize(); + AirspeedStateInitialize(); + AttitudeStateInitialize(); + TakeOffLocationInitialize(); + PoiLocationInitialize(); + ManualControlCommandInitialize(); + SystemSettingsInitialize(); + StabilizationBankInitialize(); + + // reset integrals + resetIntegrals(); // Create object queue - pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); + PathDesiredConnectCallback(SettingsUpdatedCb); + AirspeedStateConnectCallback(airspeedStateUpdatedCb); return 0; } @@ -122,17 +194,1021 @@ static void pathFollowerTask(void) FlightStatusData flightStatus; FlightStatusGet(&flightStatus); + if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + resetIntegrals(); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; } - // do pathfollower things here + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move that shit into manualcontrol! + processPOI(); + } + + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + { + uint8_t result = updateAutoPilotByFrameType(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + PathStatusSet(&pathStatus); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); } + static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - // read in settings (TODO) + FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + PathDesiredGet(&pathDesired); +} + + +static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + AirspeedStateData airspeedState; + VelocityStateData velocityState; + + AirspeedStateGet(&airspeedState); + VelocityStateGet(&velocityState); + float airspeedVector[2]; + float yaw; + AttitudeStateYawGet(&yaw); + airspeedVector[0] = cos_lookup_deg(yaw); + airspeedVector[1] = sin_lookup_deg(yaw); + // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement + float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) however + // since airspeed is updated less often than groundspeed, we use sudden + // changes to groundspeed to offset the airspeed by the same measurement. + // This has a side effect that in the absence of any airspeed updates, the + // pathfollower will fly using groundspeed. +} + + +/** + * reset integrals + */ +static void resetIntegrals() +{ + i.vel[0] = 0.0f; + i.vel[1] = 0.0f; + i.vel[2] = 0.0f; + i.course = 0.0f; + i.speed = 0.0f; + i.power = 0.0f; + i.airspeed = 0.0f; + i.poiRadius = 0.0f; + i.vtolEmergencyFallback = 0; +} + +static uint8_t updateAutoPilotByFrameType() +{ + FrameType_t frameType = GetCurrentFrameType(); + + if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) { + switch (vtolPathFollowerSettings.TreatCustomCraftAs) { + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING: + frameType = FRAME_TYPE_FIXED_WING; + break; + case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL: + frameType = FRAME_TYPE_MULTIROTOR; + break; + } + } + switch (frameType) { + case FRAME_TYPE_MULTIROTOR: + case FRAME_TYPE_HELI: + updatePeriod = vtolPathFollowerSettings.UpdatePeriod; + return updateAutoPilotVtol(); + + break; + case FRAME_TYPE_FIXED_WING: + default: + updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod; + return updateAutoPilotFixedWing(); + + break; + } +} + +/** + * fixed wing autopilot: + * straight forward: + * 1. update path velocity for limited motion crafts + * 2. update attitude according to default fixed wing pathfollower algorithm + */ +static uint8_t updateAutoPilotFixedWing() +{ + updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, true); + return updateFixedDesiredAttitude(); +} + +/** + * vtol autopilot + * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure) + * fall back to emergency fallback autopilot to keep minimum amount of flight control + */ +static uint8_t updateAutoPilotVtol() +{ + if (!i.vtolEmergencyFallback) { + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + updateVtolDesiredAttitudeEmergencyFallback(); + return 1; + } else { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false); + uint8_t result = 1; + bool yaw_attitude = true; + float yaw = 0.0f; + switch (vtolPathFollowerSettings.YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } + updateVtolDesiredAttitude(yaw_attitude, yaw); + if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { + i.vtolEmergencyFallback = true; + } + return result; + } + } else { + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + updateVtolDesiredAttitudeEmergencyFallback(); + return 0; + } +} + + +/** + * Compute bearing of current takeoff location + */ +static float updateTailInBearing() +{ + PositionStateData p; + + PositionStateGet(&p); + TakeOffLocationData t; + TakeOffLocationGet(&t); + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + +/** + * Compute bearing of current movement direction + */ +static float updateCourseBearing() +{ + VelocityStateData v; + + VelocityStateGet(&v); + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(v.East, v.North)); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + +/** + * Compute bearing between current position and POI + */ +static float updatePOIBearing() +{ + PoiLocationData poi; + + PoiLocationGet(&poi); + PositionStateData positionState; + PositionStateGet(&positionState); + + float dT = updatePeriod / 1000.0f; + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } + + return yaw + (pathAngle / 2.0f); +} + +/** + * process POI control logic TODO: this should most likely go into manualcontrol!!!! + * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently + **/ +static void processPOI() +{ + float dT = updatePeriod / 1000.0f; + + + PositionStateData positionState; + + PositionStateGet(&positionState); + // CameraDesiredData cameraDesired; + // CameraDesiredGet(&cameraDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + PoiLocationData poi; + PoiLocationGet(&poi); + + float dLoc[3]; + float yaw = 0; + /*float elevation = 0;*/ + + dLoc[0] = positionState.North - poi.North; + dLoc[1] = positionState.East - poi.East; + dLoc[2] = positionState.Down - poi.Down; + + if (dLoc[1] < 0) { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f; + } else { + yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f; + } + + // distance + float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f)); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + float changeRadius = 0; + // Move closer or further, radially + if (manualControlData.Pitch > DEADBAND_HIGH) { + changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f; + } else if (manualControlData.Pitch < DEADBAND_LOW) { + changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f; + } + + // move along circular path + float pathAngle = 0; + if (manualControlData.Roll > DEADBAND_HIGH) { + pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f; + } else if (manualControlData.Roll < DEADBAND_LOW) { + pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; + } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { + // change radius only when not circling + i.poiRadius = distance + changeRadius; + } + + // don't try to move any closer + if (i.poiRadius >= 3.0f || changeRadius > 0) { + if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { + pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (i.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.StartingVelocity = 1.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } + } + // not above + if (distance >= 3.0f) { + // You can feed this into camerastabilization + /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ + + // cameraDesired.Yaw=yaw; + // cameraDesired.PitchOrServo2=elevation; + + // CameraDesiredSet(&cameraDesired); + } +} + +/** + * Compute desired velocity from the current position and path + */ +static void updatePathVelocity(float kFF, float kH, float kV, bool limited) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); + + float groundspeed; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + boundf(progress.fractional_progress, 0.0f, 1.0f); + break; + } + // make sure groundspeed is not zero + if (limited && groundspeed < 1e-6f) { + groundspeed = 1e-6f; + } + + // calculate velocity - can be zero if waypoints are too close + VelocityDesiredData velocityDesired; + velocityDesired.North = progress.path_direction[0]; + velocityDesired.East = progress.path_direction[1]; + velocityDesired.Down = progress.path_direction[2]; + + float error_speed_horizontal = progress.error * kH; + float error_speed_vertical = progress.error * kV; + + if (limited && + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocitystate >90 degrees and + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + // calculating angles < 90 degrees through dot products + ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) && + ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) { + error_speed_horizontal = 0.0f; + error_speed_vertical = 0.0f; + } + + // calculate correction - can also be zero if correction vector is 0 or no error present + velocityDesired.North += progress.correction_direction[0] * error_speed_horizontal; + velocityDesired.East += progress.correction_direction[1] * error_speed_horizontal; + velocityDesired.Down += progress.correction_direction[2] * error_speed_vertical; + + // scale to correct length + float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down); + if (l > 1e-9f) { + velocityDesired.North *= groundspeed / l; + velocityDesired.East *= groundspeed / l; + velocityDesired.Down *= groundspeed / l; + } else { + velocityDesired.North = 0.0f; + velocityDesired.East = 0.0f; + velocityDesired.Down = 0.0f; + } + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_direction[0]; + pathStatus.path_direction_east = progress.path_direction[1]; + pathStatus.path_direction_down = progress.path_direction[2]; + + pathStatus.correction_direction_north = progress.correction_direction[0]; + pathStatus.correction_direction_east = progress.correction_direction[1]; + pathStatus.correction_direction_down = progress.correction_direction[2]; + + VelocityDesiredSet(&velocityDesired); +} + + +/** + * Compute desired attitude from the desired velocity for fixed wing craft + */ +static uint8_t updateFixedDesiredAttitude() +{ + uint8_t result = 1; + + const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s] + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + FixedWingPathFollowerStatusData fixedWingPathFollowerStatus; + AirspeedStateData airspeedState; + SystemSettingsData systemSettings; + + float groundspeedProjection; + float indicatedAirspeedState; + float indicatedAirspeedDesired; + float airspeedError; + + float pitchCommand; + + float descentspeedDesired; + float descentspeedError; + float powerCommand; + + float airspeedVector[2]; + float fluidMovement[2]; + float courseComponent[2]; + float courseError; + float courseCommand; + + FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus); + + VelocityStateGet(&velocityState); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + AirspeedStateGet(&airspeedState); + SystemSettingsGet(&systemSettings); + + /** + * Compute speed error and course + */ + // missing sensors for airspeed-direction we have to assume within + // reasonable error that measured airspeed is actually the airspeed + // component in forward pointing direction + // airspeedVector is normalized + airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw); + airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw); + + // current ground speed projected in forward direction + groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; + + // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement, + // but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias; + + // fluidMovement is a vector describing the aproximate movement vector of + // the surrounding fluid in 2d space (aka wind vector) + fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]); + fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]); + + // calculate the movement vector we need to fly to reach velocityDesired - + // taking fluidMovement into account + courseComponent[0] = velocityDesired.North - fluidMovement[0]; + courseComponent[1] = velocityDesired.East - fluidMovement[1]; + + indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]), + fixedWingPathFollowerSettings.HorizontalVelMin, + fixedWingPathFollowerSettings.HorizontalVelMax); + + // if we could fly at arbitrary speeds, we'd just have to move towards the + // courseComponent vector as previously calculated and we'd be fine + // unfortunately however we are bound by min and max air speed limits, so + // we need to recalculate the correct course to meet at least the + // velocityDesired vector direction at our current speed + // this overwrites courseComponent + bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired); + + // Error condition: wind speed too high, we can't go where we want anymore + fixedWingPathFollowerStatus.Errors.Wind = 0; + if ((!valid) && + fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Wind = 1; + result = 0; + } + + // Airspeed error + airspeedError = indicatedAirspeedDesired - indicatedAirspeedState; + + // Vertical speed error + descentspeedDesired = boundf( + velocityDesired.Down, + -fixedWingPathFollowerSettings.VerticalVelMax, + fixedWingPathFollowerSettings.VerticalVelMax); + descentspeedError = descentspeedDesired - velocityState.Down; + + // Error condition: plane too slow or too fast + fixedWingPathFollowerStatus.Errors.Highspeed = 0; + fixedWingPathFollowerStatus.Errors.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) { + fixedWingPathFollowerStatus.Errors.Overspeed = 1; + result = 0; + } + if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) { + fixedWingPathFollowerStatus.Errors.Highspeed = 1; + result = 0; + } + if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) { + fixedWingPathFollowerStatus.Errors.Lowspeed = 1; + result = 0; + } + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) { + fixedWingPathFollowerStatus.Errors.Stallspeed = 1; + result = 0; + } + + /** + * Compute desired thrust command + */ + // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. + if (fixedWingPathFollowerSettings.PowerPI.Ki > 0.0f) { + i.power = boundf(i.power + -descentspeedError * dT, + -fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki, + fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki + ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); + } else { + i.power = 0.0f; + } + + // Compute the cross feed from vertical speed to pitch, with saturation + float speedErrorToPowerCommandComponent = boundf( + (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp, + -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max, + fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max + ); + + // Compute final thrust response + powerCommand = -descentspeedError * fixedWingPathFollowerSettings.PowerPI.Kp + + i.power * fixedWingPathFollowerSettings.PowerPI.Ki + + speedErrorToPowerCommandComponent; + + // Output internal state to telemetry + fixedWingPathFollowerStatus.Error.Power = descentspeedError; + fixedWingPathFollowerStatus.ErrorInt.Power = i.power; + fixedWingPathFollowerStatus.Command.Power = powerCommand; + + // set thrust + stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand, + fixedWingPathFollowerSettings.ThrustLimit.Min, + fixedWingPathFollowerSettings.ThrustLimit.Max); + + // Error condition: plane cannot hold altitude at current speed. + fixedWingPathFollowerStatus.Errors.Lowpower = 0; + if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError > 0.0f && // we are too slow already + fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Lowpower = 1; + result = 0; + } + // Error condition: plane keeps climbing despite minimum thrust (opposite of above) + fixedWingPathFollowerStatus.Errors.Highpower = 0; + if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum + velocityState.Down < 0.0f && // we ARE going up + descentspeedDesired > 0.0f && // we WANT to go down + airspeedError < 0.0f && // we are too fast already + fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Highpower = 1; + result = 0; + } + + + /** + * Compute desired pitch command + */ + if (fixedWingPathFollowerSettings.SpeedPI.Ki > 0) { + // Integrate with saturation + i.airspeed = boundf(i.airspeed + airspeedError * dT, + -fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki, + fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki); + } + + // Compute the cross feed from vertical speed to pitch, with saturation + float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, + -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, + fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max + ); + + // Compute the pitch command as err*Kp + errInt*Ki + X_feed. + pitchCommand = -(airspeedError * fixedWingPathFollowerSettings.SpeedPI.Kp + + i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki + ) + verticalSpeedToPitchCommandComponent; + + fixedWingPathFollowerStatus.Error.Speed = airspeedError; + fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed; + fixedWingPathFollowerStatus.Command.Speed = pitchCommand; + + stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, + fixedWingPathFollowerSettings.PitchLimit.Min, + fixedWingPathFollowerSettings.PitchLimit.Max); + + // Error condition: high speed dive + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0; + if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up + velocityState.Down > 0.0f && // we ARE going down + descentspeedDesired < 0.0f && // we WANT to go up + airspeedError < 0.0f && // we are too fast already + fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on + fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1; + result = 0; + } + + /** + * Compute desired roll command + */ + courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw; + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + // overlap calculation. Theres a dead zone behind the craft where the + // counter-yawing of some craft while rolling could render a desired right + // turn into a desired left turn. Making the turn direction based on + // current roll angle keeps the plane committed to a direction once chosen + if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll > 0.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f) + && attitudeState.Roll < 0.0f) { + courseError -= 360.0f; + } + + i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki, + -fixedWingPathFollowerSettings.CoursePI.ILimit, + fixedWingPathFollowerSettings.CoursePI.ILimit); + courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp + + i.course); + + fixedWingPathFollowerStatus.Error.Course = courseError; + fixedWingPathFollowerStatus.ErrorInt.Course = i.course; + fixedWingPathFollowerStatus.Command.Course = courseCommand; + + stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + + courseCommand, + fixedWingPathFollowerSettings.RollLimit.Min, + fixedWingPathFollowerSettings.RollLimit.Max); + + // TODO: find a check to determine loss of directional control. Likely needs some check of derivative + + + /** + * Compute desired yaw command + */ + // TODO implement raw control mode for yaw and base on Accels.Y + stabDesired.Yaw = 0.0f; + + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + + StabilizationDesiredSet(&stabDesired); + + FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus); + + return result; +} + + +/** + * Function to calculate course vector C based on airspeed s, fluid movement F + * and desired movement vector V + * parameters in: V,F,s + * parameters out: C + * returns true if a valid solution could be found for V,F,s, false if not + * C will be set to a best effort attempt either way + */ +static bool correctCourse(float *C, float *V, float *F, float s) +{ + // Approach: + // Let Sc be a circle around origin marking possible movement vectors + // of the craft with airspeed s (all possible options for C) + // Let Vl be a line through the origin along movement vector V where fr any + // point v on line Vl v = k * (V / |V|) = k' * V + // Let Wl be a line parallel to Vl where for any point v on line Vl exists + // a point w on WL with w = v - F + // Then any intersection between circle Sc and line Wl represents course + // vector which would result in a movement vector + // V' = k * ( V / |V|) = k' * V + // If there is no intersection point, S is insufficient to compensate + // for F and we can only try to fly in direction of V (thus having wind drift + // but at least making progress orthogonal to wind) + + s = fabsf(s); + float f = vector_lengthf(F, 2); + + // normalize Cn=V/|V|, |V| must be >0 + float v = vector_lengthf(V, 2); + if (v < 1e-6f) { + // if |V|=0, we aren't supposed to move, turn into the wind + // (this allows hovering) + C[0] = -F[0]; + C[1] = -F[1]; + // if desired airspeed matches fluidmovement a hover is actually + // intended so return true + return fabsf(f - s) < 1e-3f; + } + float Vn[2] = { V[0] / v, V[1] / v }; + + // project F on V + float fp = F[0] * Vn[0] + F[1] * Vn[1]; + + // find component Fo of F that is orthogonal to V + // (which is exactly the distance between Vl and Wl) + float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) }; + float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1]; + + // find k where k * Vn = C - Fo + // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo + // so k^2 + fo^2 = s^2 (since |Vn|=1) + float k2 = s * s - fo2; + if (k2 <= -1e-3f) { + // there is no solution, we will be drifted off either way + // fallback: fly stupidly in direction of V and hope for the best + C[0] = V[0]; + C[1] = V[1]; + return false; + } else if (k2 <= 1e-3f) { + // there is exactly one solution: -Fo + C[0] = -Fo[0]; + C[1] = -Fo[1]; + return true; + } + // we have two possible solutions k positive and k negative as there are + // two intersection points between Wl and Sc + // which one is better? two criteria: + // 1. we MUST move in the right direction, if any k leads to -v its invalid + // 2. we should minimize the speed error + float k = sqrt(k2); + float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] }; + float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] }; + // project C+F on Vn to find signed resulting movement vector length + float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1]; + float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1]; + if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) { + // in this case the angle between course and resulting movement vector + // is greater than 90 degrees - so we actually fly backwards + C[0] = C1[0]; + C[1] = C1[1]; + return true; + } + C[0] = C2[0]; + C[1] = C2[1]; + if (vp2 >= 0.0f) { + // in this case the angle between course and movement vector is less than + // 90 degrees, but we do move in the right direction + return true; + } else { + // in this case we actually get driven in the opposite direction of V + // with both solutions for C + // this might be reached in headwind stronger than maximum allowed + // airspeed. + return false; + } +} + +/** + * Compute desired attitude from the desired velocity + * + * Takes in @ref NedState which has the acceleration in the + * NED frame as the feedback term and then compares the + * @ref VelocityState against the @ref VelocityDesired + */ +static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) +{ + float dT = updatePeriod / 1000.0f; + uint8_t result = 1; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + AttitudeStateData attitudeState; + StabilizationBankData stabSettings; + SystemSettingsData systemSettings; + + float northError; + float northCommand; + + float eastError; + float eastCommand; + + float downError; + float downCommand; + + SystemSettingsGet(&systemSettings); + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + StabilizationDesiredGet(&stabDesired); + VelocityDesiredGet(&velocityDesired); + AttitudeStateGet(&attitudeState); + StabilizationBankGet(&stabSettings); + + // Testing code - refactor into manual control command + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + // Compute desired north command + northError = velocityDesired.North - velocityState.North; + i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, + -vtolPathFollowerSettings.HorizontalVelPI.ILimit, + vtolPathFollowerSettings.HorizontalVelPI.ILimit); + northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0] + + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward); + + // Compute desired east command + eastError = velocityDesired.East - velocityState.East; + i.vel[1] = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, + -vtolPathFollowerSettings.HorizontalVelPI.ILimit, + vtolPathFollowerSettings.HorizontalVelPI.ILimit); + eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1] + + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward); + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, + -vtolPathFollowerSettings.VerticalVelPI.ILimit, + vtolPathFollowerSettings.VerticalVelPI.ILimit); + downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in + if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) { + attitudeState.Yaw += 120.0f; + if (attitudeState.Yaw > 180.0f) { + attitudeState.Yaw -= 360.0f; + } + } + + if ( // emergency flyaway detection + (fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit + velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees + ) { + result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + } + + // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the + // craft should move similarly for 5 deg roll versus 5 deg pitch + stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), + -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + if (yaw_attitude) { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.Yaw = yaw_direction; + } else { + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; + } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); + + return result; +} + +/** + * Compute desired attitude for vtols - emergency fallback + */ +static void updateVtolDesiredAttitudeEmergencyFallback() +{ + float dT = updatePeriod / 1000.0f; + + VelocityDesiredData velocityDesired; + VelocityStateData velocityState; + StabilizationDesiredData stabDesired; + + float courseError; + float courseCommand; + + float downError; + float downCommand; + + VelocityStateGet(&velocityState); + VelocityDesiredGet(&velocityDesired); + + ManualControlCommandData manualControlData; + ManualControlCommandGet(&manualControlData); + + courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); + + if (courseError < -180.0f) { + courseError += 360.0f; + } + if (courseError > 180.0f) { + courseError -= 360.0f; + } + + + courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP); + + stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max); + + // Compute desired down command + downError = velocityDesired.Down - velocityState.Down; + // Must flip this sign + downError = -downError; + i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, + -vtolPathFollowerSettings.VerticalVelPI.ILimit, + vtolPathFollowerSettings.VerticalVelPI.ILimit); + downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + + stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + + + stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll; + stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch; + + if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); + stabDesired.Thrust = manualControl.Thrust; + } + + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + StabilizationDesiredSet(&stabDesired); +} + + +/** + * Compute desired attitude from a fixed preset + * + */ +static void updateFixedAttitude(float *attitude) +{ + StabilizationDesiredData stabDesired; + + StabilizationDesiredGet(&stabDesired); + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Thrust = attitude[3]; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; + StabilizationDesiredSet(&stabDesired); } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 7cb3f27db..8d8b753b7 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -372,6 +372,7 @@ static void updatePathVelocity() float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); PositionStateData positionState; PositionStateGet(&positionState); @@ -403,7 +404,7 @@ static void updatePathVelocity() case PATHDESIRED_MODE_DRIVEVECTOR: default: speed = pathDesired.StartingVelocity - + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); + + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1); if (progress.fractional_progress > 1) { speed = 0; } @@ -416,19 +417,19 @@ static void updatePathVelocity() eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT; downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT; - northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.ILimit); - downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, - vtolpathfollowerSettings.VerticalPosPI.ILimit); + northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral + - progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral + - progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; + progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral + - progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; + progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp; // Make sure the desired velocities don't exceed PathFollower limits. float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2)); @@ -441,7 +442,7 @@ static void updatePathVelocity() velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; pathStatus.path_direction_north = progress.path_direction[0]; pathStatus.path_direction_east = progress.path_direction[1]; diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 8a7d9991a..da7ebe4fa 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -1,18 +1,23 @@ Settings for the @ref VtolPathFollowerModule - - - - - - - + + + + + + + + + - - + + + + + - + From f569a9d8cbb53e72b3ad4f450ebc20f1aa9637ff Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sun, 10 Aug 2014 20:19:29 +0200 Subject: [PATCH 114/718] OP-1397 Filter out unnecessary debug messages and use asserts instead. --- .../src/plugins/config/configtxpidwidget.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp index af3112385..0f0e7fa87 100644 --- a/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp @@ -260,8 +260,11 @@ static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, in case TxPIDSettings::PIDS_YAWATTITUDERESP: return bank->getYawMax(); + case -1: // The PID Option field was uninitialized. + return 0.0f; + default: - qDebug() << "getDefaultValueForOption: Incorrect PID option" << pidOption; + Q_ASSERT_X(false, "getDefaultValueForOption", "Incorrect PID option"); return 0.0f; } } @@ -273,7 +276,14 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption) return stab->getGyroTau(); } - uint bankNumber = m_txpid->pidBank->currentIndex() + 1; + int pidBankIndex = m_txpid->pidBank->currentIndex(); + + if (pidBankIndex == -1) { + // The pidBank field was uninitilized. + return 0.0f; + } + + int bankNumber = pidBankIndex + 1; if (bankNumber == 1) { StabilizationSettingsBank1 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank1"))); @@ -285,7 +295,7 @@ float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption) StabilizationSettingsBank3 *bank = qobject_cast(getObject(QString("StabilizationSettingsBank3"))); return defaultValueForPidOption(bank, pidOption); } else { - qDebug() << "getDefaultValueForPidOption: Incorrect bank number:" << bankNumber; + Q_ASSERT_X(false, "getDefaultValueForPidOption", "Incorrect bank number"); return 0.0f; } } @@ -307,7 +317,7 @@ void ConfigTxPIDWidget::updateSpinBoxProperties(int selectedPidOption) minPID = m_txpid->MinPID3; maxPID = m_txpid->MaxPID3; } else { - qDebug() << "updateSpinBoxProperties: Incorrect sender object"; + Q_ASSERT_X(false, "updateSpinBoxProperties", "Incorrect sender object"); return; } From ea4e7962a67fba3c0d8476598fa1e2ad39e8a73f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 10 Aug 2014 21:17:55 +0200 Subject: [PATCH 115/718] OP-1156 fixed wrong callback ID assignment for pathfollower callback diagnostics --- flight/modules/PathFollower/pathfollower.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 93efab3c9..b5d8e9e11 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -175,7 +175,7 @@ int32_t PathFollowerInitialize() resetIntegrals(); // Create object queue - pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); From 61d45181a67175b2968ad5ab479e6dd66faa1729 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Tue, 12 Aug 2014 07:37:23 +0200 Subject: [PATCH 116/718] OP-1398 OP-1399 OP-1400 OP-1401 Qt 5.3.1 upgrade. Cleaning up merge artifacts committed by mistake. --- make/tools.mk | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index d08d9d5d3..af1e76f16 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -83,13 +83,8 @@ else ifeq ($(UNAME), Windows) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip/+md5 QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe -<<<<<<< HEAD QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe.md5 - QT_SDK_ARCH := mingw48_32 -======= - QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe.md5 QT_SDK_ARCH := mingw482_32 ->>>>>>> origin/shared/OP-1398_qt531_upgrade NSIS_URL := http://wiki.openpilot.org/download/attachments/18612236/nsis-2.46-unicode.tar.bz2 SDL_URL := http://wiki.openpilot.org/download/attachments/18612236/SDL-devel-1.2.15-mingw32.tar.gz OPENSSL_URL := http://wiki.openpilot.org/download/attachments/18612236/openssl-1.0.1e-win32.tar.bz2 @@ -103,14 +98,11 @@ GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0 # Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1 QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1 -<<<<<<< HEAD MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32 PYTHON_DIR := $(QT_SDK_DIR)/Tools/mingw48_32/opt/bin NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode SDL_DIR := $(TOOLS_DIR)/SDL-1.2.15 OPENSSL_DIR := $(TOOLS_DIR)/openssl-1.0.1e-win32 -======= ->>>>>>> origin/shared/OP-1398_qt531_upgrade UNCRUSTIFY_DIR := $(TOOLS_DIR)/uncrustify-0.60 DOXYGEN_DIR := $(TOOLS_DIR)/doxygen-1.8.3.1 GTEST_DIR := $(TOOLS_DIR)/gtest-1.6.0 @@ -369,16 +361,6 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) $(DL_DIR)/$(5) --dump-binary-data -o $(1) # Extract packages under tool directory $(V1) $(MKDIR) -p $$(call toprel, $(dir $(2))) -<<<<<<< HEAD - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1mingw48_essentials.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1x32-4.8.0-release-posix-dwarf-rev2-runtime.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.essentials/5.3.1icu_51_1_mingw_builds_4_8_0_posix_dwarf_32.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw48.addons/5.3.1mingw48_addons.7z" | grep -v Extracting - $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw48/4.8.0-1-1x32-4.8.0-release-posix-dwarf-rev2.7z" | grep -v Extracting -======= $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting @@ -387,7 +369,6 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0icu_52_1_mingw_builds_32_4_8_2_posix_dwarf.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0qt5_addons.7z" | grep -v Extracting $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw482/4.8.2i686-4.8.2-release-posix-dwarf-rt_v3-rev3.7z" | grep -v Extracting ->>>>>>> origin/shared/OP-1398_qt531_upgrade # Run patcher @$(ECHO) @$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX)) From 1f74977472e6ba1ca82c178f7d625fc6cda9f87c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 12 Aug 2014 09:44:44 +0200 Subject: [PATCH 117/718] OP-1379 add base types for colour, Initial ext rgb led api definition --- flight/libraries/inc/optypes.h | 61 +++++++++++++++++++++++++++++ flight/libraries/optypes.c | 40 +++++++++++++++++++ flight/pios/common/pios_notify.c | 29 +++++++++++++- flight/pios/inc/pios_notify.h | 50 +++++++++++++++++++++-- flight/pios/inc/pios_ws2811.h | 11 +----- flight/pios/stm32f4xx/pios_ws2811.c | 4 +- make/apps-defs.mk | 1 + 7 files changed, 181 insertions(+), 15 deletions(-) create mode 100644 flight/libraries/inc/optypes.h create mode 100644 flight/libraries/optypes.c diff --git a/flight/libraries/inc/optypes.h b/flight/libraries/inc/optypes.h new file mode 100644 index 000000000..d48e87887 --- /dev/null +++ b/flight/libraries/inc/optypes.h @@ -0,0 +1,61 @@ +/** + ****************************************************************************** + * + * @file optypes.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief OP Generic data type library + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 UTIL_H +#define UTIL_H +#include +typedef struct { + uint8_t R; + uint8_t G; + uint8_t B; +} Color_t; + +extern const Color_t Color_Off; +extern const Color_t Color_Red; +extern const Color_t Color_Lime; +extern const Color_t Color_Blue; +extern const Color_t Color_Yellow; +extern const Color_t Color_Cian; +extern const Color_t Color_Magenta; +extern const Color_t Color_Navy; +extern const Color_t Color_Green; +extern const Color_t Color_Purple; +extern const Color_t Color_Teal; +extern const Color_t Color_Orange; + +#define COLOR_OFF { .R = 0x00, .G = 0x00, .B = 0x00 } +#define COLOR_RED { .R = 0xFF, .G = 0x00, .B = 0x00 } +#define COLOR_LIME { .R = 0x00, .G = 0xFF, .B = 0x00 } +#define COLOR_BLUE { .R = 0x00, .G = 0x00, .B = 0xFF } +#define COLOR_YELLOW { .R = 0xFF, .G = 0xFF, .B = 0x00 } +#define COLOR_CIAN { .R = 0x00, .G = 0xFF, .B = 0xFF } +#define COLOR_MAGENTA { .R = 0xFF, .G = 0x00, .B = 0xFF } +#define COLOR_NAVY { .R = 0x00, .G = 0x00, .B = 0x80 } +#define COLOR_GREEN { .R = 0x00, .G = 0x80, .B = 0x00 } +#define COLOR_PURPLE { .R = 0x80, .G = 0x00, .B = 0x80 } +#define COLOR_TEAL { .R = 0x00, .G = 0x80, .B = 0x80 } +#define COLOR_ORANGE { .R = 0xFF, .G = 0xA5, .B = 0x00 } + +#endif /* UTIL_H */ diff --git a/flight/libraries/optypes.c b/flight/libraries/optypes.c new file mode 100644 index 000000000..a203ee555 --- /dev/null +++ b/flight/libraries/optypes.c @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * + * @file optypes.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief OP Generic data type library + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 + +const Color_t Color_Off = COLOR_OFF; +const Color_t Color_Red = COLOR_RED; +const Color_t Color_Lime = COLOR_LIME; +const Color_t Color_Blue = COLOR_BLUE; +const Color_t Color_Yellow = COLOR_YELLOW; +const Color_t Color_Cian = COLOR_CIAN; +const Color_t Color_Magenta = COLOR_MAGENTA; +const Color_t Color_Navy = COLOR_NAVY; +const Color_t Color_Green = COLOR_GREEN; +const Color_t Color_Purple = COLOR_PURPLE; +const Color_t Color_Teal = COLOR_TEAL; +const Color_t Color_Orange = COLOR_ORANGE; diff --git a/flight/pios/common/pios_notify.c b/flight/pios/common/pios_notify.c index bf83c22eb..5f47a18a6 100644 --- a/flight/pios/common/pios_notify.c +++ b/flight/pios/common/pios_notify.c @@ -28,7 +28,8 @@ static volatile pios_notify_notification currentNotification = NOTIFY_NONE; static volatile pios_notify_priority currentPriority; - +static volatile ExtLedNotification_t extLedNotification; +static volatile bool newNotification; void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority) { @@ -47,3 +48,29 @@ pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear) } return ret; } + + +/* + * Play a sequence on the default external led. Sequences with priority higher than NOTIFY_PRIORITY_LOW + * are repeated only once if repeat = -1 + * @param sequence Sequence to be played + * @param priority Priority of the sequence being played + */ +void PIOS_NOTIFICATION_Default_Ext_Led_Play(const LedSequence_t *sequence, pios_notify_priority priority) +{ + extLedNotification.priority = priority; + extLedNotification.sequence = *sequence; + newNotification = true; +} + + +ExtLedNotification_t *PIOS_NOTIFY_GetNewExtLedSequence(bool clear) +{ + if (!newNotification) { + return 0; + } + if (clear) { + newNotification = false; + } + return (ExtLedNotification_t *)&extLedNotification; +} diff --git a/flight/pios/inc/pios_notify.h b/flight/pios/inc/pios_notify.h index 1e800df5c..12daefdc2 100644 --- a/flight/pios/inc/pios_notify.h +++ b/flight/pios/inc/pios_notify.h @@ -26,7 +26,7 @@ #ifndef PIOS_NOTIFY_H_ #define PIOS_NOTIFY_H_ #include - +#include typedef enum { NOTIFY_NONE = 0, NOTIFY_OK, @@ -35,11 +35,35 @@ typedef enum { } pios_notify_notification; typedef enum { - NOTIFY_PRIORITY_CRITICAL = 2, - NOTIFY_PRIORITY_REGULAR = 1, + NOTIFY_PRIORITY_CRITICAL = 2, + NOTIFY_PRIORITY_REGULAR = 1, NOTIFY_PRIORITY_LOW = 0, + NOTIFY_PRIORITY_BACKGROUND = -1, } pios_notify_priority; + +// A single led step, Color, time On/Off, repeat count +#define NOTIFY_SEQUENCE_MAX_STEPS 5 + +typedef struct { + uint16_t time_off; + uint16_t time_on; + Color_t color; + uint8_t repeats; +} LedStep_t; + +typedef struct { + int8_t repeats; // -1 for infinite repetitions + LedStep_t steps[NOTIFY_SEQUENCE_MAX_STEPS]; +} LedSequence_t; + +typedef struct { + LedSequence_t sequence; + pios_notify_priority priority; +} ExtLedNotification_t; + +#define NOTIFY_IS_NULL_STEP(x) (!x || (!x->time_off && !x->time_on && !x->repeats)) + /** * start a new notification. If a notification is active it will be overwritten if its priority is lower than the new one. * The new will be discarded otherwise @@ -55,4 +79,24 @@ void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_n */ pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear); +/* + * Play a sequence on the default external led. Sequences with priority higher than NOTIFY_PRIORITY_LOW + * are repeated only once if repeat = -1 + * @param sequence Sequence to be played + * @param priority Priority of the sequence being played + */ +void PIOS_NOTIFICATION_Default_Ext_Led_Play(const LedSequence_t *sequence, pios_notify_priority priority); + +/* + * Play a sequence on an external rgb led. Sequences with priority higher than NOTIFY_PRIORITY_LOW + * are repeated only once if repeat = -1 + * @param sequence Sequence to be played + * @param ledNumber Led number + * @param priority Priority of the sequence being played + * + */ +// void PIOS_NOTIFICATION_Ext_Led_Play(const LedSequence_t sequence, uint8_t ledNumber, pios_notify_priority priority); + +ExtLedNotification_t *PIOS_NOTIFY_GetNewExtLedSequence(bool clear); + #endif /* PIOS_NOTIFY_H_ */ diff --git a/flight/pios/inc/pios_ws2811.h b/flight/pios/inc/pios_ws2811.h index cb05070ee..6b8778b19 100644 --- a/flight/pios/inc/pios_ws2811.h +++ b/flight/pios/inc/pios_ws2811.h @@ -35,7 +35,7 @@ #include #include #include - +#include #define sign(x) ((x > 0) - (x < 0)) #define PIOS_WS2811_NUMLEDS 2 @@ -106,12 +106,6 @@ typedef uint16_t ledbuf_t; -typedef struct Color Color; -struct Color { - uint8_t R; - uint8_t G; - uint8_t B; -}; struct pios_ws2811_pin_cfg { GPIO_TypeDef *gpio; GPIO_InitTypeDef gpioInit; @@ -139,9 +133,8 @@ struct pios_ws2811_cfg { void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg); -void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update); +void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update); void PIOS_WS2811_Update(); void PIOS_WS2811_DMA_irq_handler(); - #endif /* PIOS_WS2811_H_ */ diff --git a/flight/pios/stm32f4xx/pios_ws2811.c b/flight/pios/stm32f4xx/pios_ws2811.c index 96719a4a8..f4ffe1bed 100644 --- a/flight/pios/stm32f4xx/pios_ws2811.c +++ b/flight/pios/stm32f4xx/pios_ws2811.c @@ -177,7 +177,7 @@ void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pio fb = (ledbuf_t *)pios_malloc(PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t)); memset(fb, 0, PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t)); - Color ledoff = { 0, 0, 0 }; + const Color_t ledoff = Color_Off; for (uint8_t i = 0; i < PIOS_WS2811_NUMLEDS; i++) { PIOS_WS2811_setColorRGB(ledoff, i, false); } @@ -302,7 +302,7 @@ void setColor(uint8_t color, ledbuf_t *buf) * @param led led number * @param update Perform an update after changing led color */ -void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update) +void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update) { if (led >= PIOS_WS2811_NUMLEDS) { return; diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 72381423e..1c473383c 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -110,6 +110,7 @@ SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c SRC += $(MATHLIB)/mathmisc.c SRC += $(FLIGHTLIB)/printf-stdarg.c +SRC += $(FLIGHTLIB)/optypes.c ## Modules SRC += $(foreach mod, $(MODULES), $(sort $(wildcard $(OPMODULEDIR)/$(mod)/*.c))) From 56e4127bca5c9b697fc0d32ea37de20c51960849 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 18:18:07 +0200 Subject: [PATCH 118/718] OP-1156: add path_direction as a source for yaw as suggested by Werner :) --- flight/modules/PathFollower/pathfollower.c | 33 +++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index b5d8e9e11..0ffa2c741 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -122,6 +122,7 @@ static uint8_t updateAutoPilotFixedWing(); static uint8_t updateAutoPilotVtol(); static float updateTailInBearing(); static float updateCourseBearing(); +static float updatePathBearing(); static float updatePOIBearing(); static void processPOI(); static void updatePathVelocity(float kFF, float kH, float kV, bool limited); @@ -355,9 +356,12 @@ static uint8_t updateAutoPilotVtol() case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: yaw = updateTailInBearing(); break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE: + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: yaw = updateCourseBearing(); break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: yaw = updatePOIBearing(); break; @@ -412,6 +416,33 @@ static float updateCourseBearing() return yaw; } +/** + * Compute bearing of current path direction + */ +static float updatePathBearing() +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + + float cur[3] = { positionState.North, + positionState.East, + positionState.Down }; + struct path_status progress; + + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); + + // atan2f always returns in between + and - 180 degrees + float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0])); + // result is in between 0 and 360 degrees + if (yaw < 0.0f) { + yaw += 360.0f; + } + return yaw; +} + /** * Compute bearing between current position and POI */ From 4fc071978c694f2f40d656d93f64d266ab842e81 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 18:23:45 +0200 Subject: [PATCH 119/718] OP-1156 commit changed xml file --- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index da7ebe4fa..ebc6d9520 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -12,7 +12,7 @@ - + From 09e8b384070a3068580be55f3f1f47b77950b508 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 12 Aug 2014 19:12:05 +0200 Subject: [PATCH 120/718] OP-1156 fixed EmergencyFlyaway detection algorithm --- flight/modules/PathFollower/pathfollower.c | 29 ++++++++++++++----- .../vtolpathfollowersettings.xml | 1 + 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 0ffa2c741..6db2c2759 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -96,7 +96,8 @@ struct Integrals { float power; float airspeed; float poiRadius; - bool vtolEmergencyFallback; + float vtolEmergencyFallback; + bool vtolEmergencyFallbackSwitch; }; @@ -288,6 +289,7 @@ static void resetIntegrals() i.airspeed = 0.0f; i.poiRadius = 0.0f; i.vtolEmergencyFallback = 0; + i.vtolEmergencyFallbackSwitch = false; } static uint8_t updateAutoPilotByFrameType() @@ -339,7 +341,7 @@ static uint8_t updateAutoPilotFixedWing() */ static uint8_t updateAutoPilotVtol() { - if (!i.vtolEmergencyFallback) { + if (!i.vtolEmergencyFallbackSwitch) { if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); updateVtolDesiredAttitudeEmergencyFallback(); @@ -366,9 +368,9 @@ static uint8_t updateAutoPilotVtol() yaw = updatePOIBearing(); break; } - updateVtolDesiredAttitude(yaw_attitude, yaw); + result = updateVtolDesiredAttitude(yaw_attitude, yaw); if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { - i.vtolEmergencyFallback = true; + i.vtolEmergencyFallbackSwitch = true; } return result; } @@ -1120,10 +1122,23 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) } if ( // emergency flyaway detection - (fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit - velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees + ( // integral already at its limit + vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[0]) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[1]) < 1e-6f + ) && + // angle between desired and actual velocity >90 degrees (by dot product) + (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && + // quad is moving at significant speed (during flyaway it would keep speeding up) + squaref(velocityState.North) + squaref(velocityState.East) > 1.0f ) { - result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + i.vtolEmergencyFallback += dT; + if (i.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { + // after emergency timeout, trigger alarm - everything else is handled by callers + // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) + result = 0; + } + } else { + i.vtolEmergencyFallback = 0.0f; } // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index ebc6d9520..7da8f9091 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -14,6 +14,7 @@ + From fd92fdfadf20b0f5f53b81c867088d8cd32c6808 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 12 Aug 2014 22:18:36 +0200 Subject: [PATCH 121/718] OP-1379 - initial version of external led sequencer implemented in notification lib. Alert overlay not working yet. --- flight/libraries/inc/notification.h | 1 + flight/libraries/inc/optypes.h | 3 +- flight/libraries/notification.c | 255 +++++++++++++++++++++++++++- flight/libraries/optypes.c | 1 + 4 files changed, 256 insertions(+), 4 deletions(-) diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h index 2053a9609..bcf85015c 100644 --- a/flight/libraries/inc/notification.h +++ b/flight/libraries/inc/notification.h @@ -25,6 +25,7 @@ */ #ifndef NOTIFICATION_H #define NOTIFICATION_H +#include // period of each blink phase #define LED_BLINK_PERIOD_MS 200 diff --git a/flight/libraries/inc/optypes.h b/flight/libraries/inc/optypes.h index d48e87887..82a95e52e 100644 --- a/flight/libraries/inc/optypes.h +++ b/flight/libraries/inc/optypes.h @@ -44,6 +44,7 @@ extern const Color_t Color_Green; extern const Color_t Color_Purple; extern const Color_t Color_Teal; extern const Color_t Color_Orange; +extern const Color_t Color_White; #define COLOR_OFF { .R = 0x00, .G = 0x00, .B = 0x00 } #define COLOR_RED { .R = 0xFF, .G = 0x00, .B = 0x00 } @@ -57,5 +58,5 @@ extern const Color_t Color_Orange; #define COLOR_PURPLE { .R = 0x80, .G = 0x00, .B = 0x80 } #define COLOR_TEAL { .R = 0x00, .G = 0x80, .B = 0x80 } #define COLOR_ORANGE { .R = 0xFF, .G = 0xA5, .B = 0x00 } - +#define COLOR_WHITE { .R = 0xAA, .G = 0xAA, .B = 0xAA } #endif /* UTIL_H */ diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index 9015e4dd3..c02152e02 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -29,6 +29,41 @@ #include #include #include +#include + +#ifdef PIOS_INCLUDE_WS2811 +#include +#endif +// Private defines +// Maximum number of notifications enqueued when a higher priority notification is added +#define MAX_BACKGROUND_NOTIFICATIONS 3 +#define MAX_HANDLED_LED 1 + +#define BACKGROUND_SEQUENCE -1 +#define RESET_STEP -1 +#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS) +// Private data types definition + +// this is the status for a single LED notification led set +typedef struct { + int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; + LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; + LedSequence_t background_sequence; + uint32_t next_run_time; + uint32_t sequence_starting_time; + + int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE + bool running; // is this led running? + bool step_phase_on; // true = step on phase, false = step off phase + uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) + uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) + uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) + uint8_t led_set; // target led set +} NotifierLedStatus_t; + +#ifdef PIOS_INCLUDE_WS2811 +static bool led_status_initialized = false; +#endif #ifdef PIOS_LED_ALARM #define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) @@ -137,6 +172,9 @@ static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern); static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern); static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern); static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern); + +static void HandleExtLeds(); + void NotificationUpdateStatus() { started = true; @@ -150,7 +188,7 @@ void NotificationUpdateStatus() void NotificationOnboardLedsRun() { - static portTickType lastRunTimestamp; + static uint32_t lastRunTimestamp; static uint16_t r_pattern; static uint16_t b_pattern; static uint8_t cycleCount; // count the number of cycles @@ -164,11 +202,13 @@ void NotificationOnboardLedsRun() STATUS_LENGHT } status; - if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) { + HandleExtLeds(); + const uint32_t current_timestamp = GET_CURRENT_MILLIS; + if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) { return; } - lastRunTimestamp = xTaskGetTickCount(); + lastRunTimestamp = current_timestamp; // the led will show various status information, subdivided in three phases // - Notification // - Alarm @@ -294,3 +334,212 @@ static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern) *b_pattern = BLINK_B_HEARTBEAT_PATTERN; *r_pattern = BLINK_R_HEARTBEAT_PATTERN; } + +#ifdef PIOS_INCLUDE_WS2811 + +NotifierLedStatus_t led_status[MAX_HANDLED_LED]; + +static void InitExtLed() +{ + memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); + const uint32_t now = GET_CURRENT_MILLIS; + for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { + led_status[l].led_set = l; + led_status[l].next_run_time = now + 500; // start within half a second + for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { + led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; + } + } +} + +/** + * restart current sequence + */ +static void restart_sequence(NotifierLedStatus_t *status, bool immediate) +{ + status->next_sequence_step = 0; + status->next_step_rep = 0; + status->step_phase_on = true; + status->running = true; + if (immediate) { + uint32_t currentTime = GET_CURRENT_MILLIS; + status->next_run_time = currentTime; + } + status->sequence_starting_time = status->next_run_time; +} + +/** + * modify background sequence or enqueue a new sequence to play + */ +static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status) +{ + int8_t updated_sequence = BACKGROUND_SEQUENCE; + + if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) { + status->background_sequence = new_notification->sequence; + } else { + // a notification with priority higher than background. + // try to enqueue it + int8_t insert_point = -1; + int8_t first_free = -1; + for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; i--) { + const int8_t priority_i = status->queued_priorities[i]; + if (priority_i == NOTIFY_PRIORITY_BACKGROUND) { + first_free = i; + insert_point = i; + continue; + } + if (priority_i > new_notification->priority) { + insert_point = i; + } + } + + if (insert_point != first_free) { + // there is a free slot, move everything up one place + if (first_free != -1) { + for (uint8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i > insert_point; i--) { + status->queued_priorities[i] = status->queued_priorities[i - 1]; + status->queued_sequences[i] = status->queued_sequences[i - 1]; + } + if (status->active_sequence_num >= insert_point) { + status->active_sequence_num++; + } + } else { + // no free space, discard lowest priority notification and move everything down + for (uint8_t i = 0; i < insert_point; i++) { + status->queued_priorities[i] = status->queued_priorities[i + 1]; + status->queued_sequences[i] = status->queued_sequences[i + 1]; + } + if (status->active_sequence_num <= insert_point) { + status->active_sequence_num--; + } + } + } + + status->queued_priorities[insert_point] = new_notification->priority; + status->queued_sequences[insert_point] = new_notification->sequence; + updated_sequence = insert_point; + } + + if (status->active_sequence_num < updated_sequence) { + status->active_sequence_num = updated_sequence; + restart_sequence(status, true); + } + if (updated_sequence == BACKGROUND_SEQUENCE) { + restart_sequence(status, false); + } +} + +static bool pop_queued_sequence(NotifierLedStatus_t *status) +{ + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // start the lower priority item + status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND; + status->active_sequence_num--; + return true; + } + // background sequence was completed + return false; +} + +/** + * advance current sequence pointers for next step + */ +static void advance_sequence(NotifierLedStatus_t *status) +{ + LedSequence_t *activeSequence = + status->active_sequence_num == BACKGROUND_SEQUENCE ? + &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + + uint8_t step = status->next_sequence_step; + LedStep_t *currentStep = &activeSequence->steps[step]; + + // Next step will be the OFF phase, so just update the time and + if (status->step_phase_on) { + // next will be the off phase + status->next_run_time += currentStep->time_on; + status->step_phase_on = false; + // check if off phase should be skipped + if (currentStep->time_off != 0) { + return; + } + } + + // next step is ON phase. check whether to repeat current step or move to next one + status->next_run_time += currentStep->time_off; + status->step_phase_on = true; + + if (status->next_step_rep + 1 < currentStep->repeats) { + // setup next repetition + status->next_step_rep++; + return; + } + + // move to next step + LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0; + + // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences + if (NOTIFY_IS_NULL_STEP(nextStep)) { + if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) { + status->next_sequence_rep++; + // restart the sequence + restart_sequence(status, false); + return; + } + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // no repeat, pop enqueued or background sequences + pop_queued_sequence(status); + restart_sequence(status, false); + } else { + status->running = false; + } + } else { + status->next_step_rep = 0; + status->next_sequence_step++; + } +} + +/** + * run a led set + */ +static void run_led(NotifierLedStatus_t *status) +{ + uint32_t currentTime = GET_CURRENT_MILLIS; + + if (!status->running || currentTime < status->next_run_time) { + return; + } + status->next_run_time = currentTime; + uint8_t step = status->next_sequence_step; + + LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ? + &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + if (status->step_phase_on) { + PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true); + } else { + PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); + } + advance_sequence(status); +} + +void HandleExtLeds() +{ + // handle incoming sequences + if (!led_status_initialized) { + InitExtLed(); + led_status_initialized = true; + } + static ExtLedNotification_t *newNotification; + newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true); + if (newNotification) { + push_queued_sequence(newNotification, &led_status[0]); + } + + // Run Leds + for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) { + run_led(&led_status[i]); + } +} +#else /* ifdef PIOS_INCLUDE_WS2811 */ +void HandleExtLeds() {} +#endif /* ifdef PIOS_INCLUDE_WS2811 */ diff --git a/flight/libraries/optypes.c b/flight/libraries/optypes.c index a203ee555..b67b81c3f 100644 --- a/flight/libraries/optypes.c +++ b/flight/libraries/optypes.c @@ -38,3 +38,4 @@ const Color_t Color_Green = COLOR_GREEN; const Color_t Color_Purple = COLOR_PURPLE; const Color_t Color_Teal = COLOR_TEAL; const Color_t Color_Orange = COLOR_ORANGE; +const Color_t Color_White = COLOR_WHITE; From 1067ea59d18bbe7c66a6fdb06917d518aa29c136 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 12 Aug 2014 22:19:30 +0200 Subject: [PATCH 122/718] OP-1379 - Split pios_ws2811.h in config and public api header --- flight/pios/inc/pios_ws2811.h | 103 +------------ flight/pios/inc/pios_ws2811_cfg.h | 137 ++++++++++++++++++ flight/pios/stm32f4xx/pios_ws2811.c | 2 +- .../boards/discoveryf4bare/board_hw_defs.c | 2 +- .../targets/boards/revolution/board_hw_defs.c | 2 +- 5 files changed, 141 insertions(+), 105 deletions(-) create mode 100644 flight/pios/inc/pios_ws2811_cfg.h diff --git a/flight/pios/inc/pios_ws2811.h b/flight/pios/inc/pios_ws2811.h index 6b8778b19..2d04288a2 100644 --- a/flight/pios/inc/pios_ws2811.h +++ b/flight/pios/inc/pios_ws2811.h @@ -29,112 +29,11 @@ #define PIOS_WS2811_H_ #include -#include -#include -#include -#include -#include -#include #include -#define sign(x) ((x > 0) - (x < 0)) -#define PIOS_WS2811_NUMLEDS 2 -#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24)) -#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord -#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord -#define PIOS_WS2811_TIM_PERIOD 20 - -// Following times are keept on the lower side to accounts -// for bus contentions and irq response time -#define PIOS_WS2811_T0_HIGH_PERIOD 25 // .35us +/- 150nS -#define PIOS_WS2811_T1_HIGH_PERIOD 60 // .70us +/- 150nS - -#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \ - { \ - .DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_VeryHigh, } - -#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \ - { \ - .DMA_BufferSize = 4, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_VeryHigh, } - -#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \ - { \ - .DMA_BufferSize = 4, \ - .DMA_Channel = channel, \ - .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ - .DMA_FIFOMode = DMA_FIFOMode_Enable, \ - .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ - .DMA_Memory0BaseAddr = 0, \ - .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ - .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ - .DMA_MemoryInc = DMA_MemoryInc_Enable, \ - .DMA_Mode = DMA_Mode_Circular, \ - .DMA_PeripheralBaseAddr = 0, \ - .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ - .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ - .DMA_Priority = DMA_Priority_High } - - -typedef uint16_t ledbuf_t; - -struct pios_ws2811_pin_cfg { - GPIO_TypeDef *gpio; - GPIO_InitTypeDef gpioInit; -}; -struct pios_ws2811_cfg { - TIM_TimeBaseInitTypeDef timerInit; - TIM_TypeDef *timer; - uint8_t timerCh1; - uint8_t timerCh2; - - DMA_InitTypeDef dmaInitCh1; - DMA_Stream_TypeDef *streamCh1; - uint32_t dmaItCh1; - - DMA_InitTypeDef dmaInitCh2; - DMA_Stream_TypeDef *streamCh2; - uint32_t dmaItCh2; - - DMA_InitTypeDef dmaInitUpdate; - DMA_Stream_TypeDef *streamUpdate; - uint32_t dmaItUpdate; - uint16_t dmaSource; - struct stm32_irq irq; -}; - -void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg); +#define PIOS_WS2811_NUMLEDS 2 void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update); void PIOS_WS2811_Update(); -void PIOS_WS2811_DMA_irq_handler(); #endif /* PIOS_WS2811_H_ */ diff --git a/flight/pios/inc/pios_ws2811_cfg.h b/flight/pios/inc/pios_ws2811_cfg.h new file mode 100644 index 000000000..379df77be --- /dev/null +++ b/flight/pios/inc/pios_ws2811_cfg.h @@ -0,0 +1,137 @@ +/** + ****************************************************************************** + * + * @file pios_ws2811_cfg.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief A driver for ws2811 rgb led controller. + * this is a plain PiOS port of the very clever solution + * implemented by Omri Iluz in the chibios driver here: + * https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 PIOS_WS2811_CFG_H_ +#define PIOS_WS2811_CFG_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define sign(x) ((x > 0) - (x < 0)) +#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24)) +#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord +#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord +#define PIOS_WS2811_TIM_PERIOD 20 + +// Following times are keept on the lower side to accounts +// for bus contentions and irq response time +#define PIOS_WS2811_T0_HIGH_PERIOD 25 // .35us +/- 150nS +#define PIOS_WS2811_T1_HIGH_PERIOD 60 // .70us +/- 150nS + +#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \ + { \ + .DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_VeryHigh, } + +#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \ + { \ + .DMA_BufferSize = 4, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_VeryHigh, } + +#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \ + { \ + .DMA_BufferSize = 4, \ + .DMA_Channel = channel, \ + .DMA_DIR = DMA_DIR_MemoryToPeripheral, \ + .DMA_FIFOMode = DMA_FIFOMode_Enable, \ + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \ + .DMA_Memory0BaseAddr = 0, \ + .DMA_MemoryBurst = DMA_MemoryBurst_INC4, \ + .DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \ + .DMA_MemoryInc = DMA_MemoryInc_Enable, \ + .DMA_Mode = DMA_Mode_Circular, \ + .DMA_PeripheralBaseAddr = 0, \ + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \ + .DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \ + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, \ + .DMA_Priority = DMA_Priority_High } + + +typedef uint16_t ledbuf_t; + +struct pios_ws2811_pin_cfg { + GPIO_TypeDef *gpio; + GPIO_InitTypeDef gpioInit; +}; +struct pios_ws2811_cfg { + TIM_TimeBaseInitTypeDef timerInit; + TIM_TypeDef *timer; + uint8_t timerCh1; + uint8_t timerCh2; + + DMA_InitTypeDef dmaInitCh1; + DMA_Stream_TypeDef *streamCh1; + uint32_t dmaItCh1; + + DMA_InitTypeDef dmaInitCh2; + DMA_Stream_TypeDef *streamCh2; + uint32_t dmaItCh2; + + DMA_InitTypeDef dmaInitUpdate; + DMA_Stream_TypeDef *streamUpdate; + uint32_t dmaItUpdate; + uint16_t dmaSource; + struct stm32_irq irq; +}; + +void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg); + +void PIOS_WS2811_DMA_irq_handler(); +#endif /* PIOS_WS2811_H_ */ diff --git a/flight/pios/stm32f4xx/pios_ws2811.c b/flight/pios/stm32f4xx/pios_ws2811.c index f4ffe1bed..7762cdfb5 100644 --- a/flight/pios/stm32f4xx/pios_ws2811.c +++ b/flight/pios/stm32f4xx/pios_ws2811.c @@ -29,7 +29,7 @@ #ifdef PIOS_INCLUDE_WS2811 -#include "pios_ws2811.h" +#include "pios_ws2811_cfg.h" #include #include #include "FreeRTOS.h" diff --git a/flight/targets/boards/discoveryf4bare/board_hw_defs.c b/flight/targets/boards/discoveryf4bare/board_hw_defs.c index 4583fe6b4..097a8e193 100644 --- a/flight/targets/boards/discoveryf4bare/board_hw_defs.c +++ b/flight/targets/boards/discoveryf4bare/board_hw_defs.c @@ -1811,7 +1811,7 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { }; #endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ #ifdef PIOS_INCLUDE_WS2811 -#include +#include #define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD)) void DMA2_Stream1_IRQHandler(void) __attribute__((alias("PIOS_WS2811_irq_handler"))); diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 98fce19c7..3a7cf115f 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1964,7 +1964,7 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { #endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */ #ifdef PIOS_INCLUDE_WS2811 -#include +#include #include #define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD)) From f556a5e335cb3bdb47b183468536e5012a980993 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 13 Aug 2014 15:14:55 +0200 Subject: [PATCH 123/718] OP-1379 - Split simple notification and external lednotifications libraries --- flight/libraries/inc/lednotification.h | 33 +++ flight/libraries/inc/notification.h | 1 - flight/libraries/lednotification.c | 268 ++++++++++++++++++ flight/libraries/notification.c | 244 +--------------- flight/modules/System/systemmod.c | 10 +- .../boards/discoveryf4bare/firmware/Makefile | 1 + .../boards/revolution/firmware/Makefile | 1 + 7 files changed, 314 insertions(+), 244 deletions(-) create mode 100644 flight/libraries/inc/lednotification.h create mode 100644 flight/libraries/lednotification.c diff --git a/flight/libraries/inc/lednotification.h b/flight/libraries/inc/lednotification.h new file mode 100644 index 000000000..fadd100f0 --- /dev/null +++ b/flight/libraries/inc/lednotification.h @@ -0,0 +1,33 @@ +/** + ****************************************************************************** + * + * @file lednotification.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief led notification library + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 LEDNOTIFICATION_H_ +#define LEDNOTIFICATION_H_ +#include + +void LedNotificationExtLedsRun(); + + +#endif /* LEDNOTIFICATION_H_ */ diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h index bcf85015c..2053a9609 100644 --- a/flight/libraries/inc/notification.h +++ b/flight/libraries/inc/notification.h @@ -25,7 +25,6 @@ */ #ifndef NOTIFICATION_H #define NOTIFICATION_H -#include // period of each blink phase #define LED_BLINK_PERIOD_MS 200 diff --git a/flight/libraries/lednotification.c b/flight/libraries/lednotification.c new file mode 100644 index 000000000..b2ca13597 --- /dev/null +++ b/flight/libraries/lednotification.c @@ -0,0 +1,268 @@ +/** + ****************************************************************************** + * + * @file lednotification.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief led notification library. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 "inc/lednotification.h" +#include +#include +#include +#include +#include +#include +#include + +// Private defines + +// Maximum number of notifications enqueued when a higher priority notification is added +#define MAX_BACKGROUND_NOTIFICATIONS 5 +#define MAX_HANDLED_LED 1 + +#define BACKGROUND_SEQUENCE -1 +#define RESET_STEP -1 +#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS) + +// Private data types definition +// this is the status for a single notification led set +typedef struct { + int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; + LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; + LedSequence_t background_sequence; + uint32_t next_run_time; + uint32_t sequence_starting_time; + + int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE + bool running; // is this led running? + bool step_phase_on; // true = step on phase, false = step off phase + uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) + uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) + uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) + uint8_t led_set; // target led set +} NotifierLedStatus_t; + +static bool led_status_initialized = false; + +NotifierLedStatus_t led_status[MAX_HANDLED_LED]; + +static void InitExtLed() +{ + memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); + const uint32_t now = GET_CURRENT_MILLIS; + for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { + led_status[l].led_set = 0; + led_status[l].next_run_time = now + 500; // start within half a second + for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { + led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; + } + } +} + +/** + * restart current sequence + */ +static void restart_sequence(NotifierLedStatus_t *status, bool immediate) +{ + status->next_sequence_step = 0; + status->next_step_rep = 0; + status->step_phase_on = true; + status->running = true; + if (immediate) { + uint32_t currentTime = GET_CURRENT_MILLIS; + status->next_run_time = currentTime; + } + status->sequence_starting_time = status->next_run_time; +} + +/** + * modify background sequence or enqueue a new sequence to play + */ +static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status) +{ + int8_t updated_sequence = BACKGROUND_SEQUENCE; + + if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) { + status->background_sequence = new_notification->sequence; + } else { + // a notification with priority higher than background. + // try to enqueue it + int8_t insert_point = -1; + int8_t first_free = -1; + for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; i--) { + const int8_t priority_i = status->queued_priorities[i]; + if (priority_i == NOTIFY_PRIORITY_BACKGROUND) { + first_free = i; + insert_point = i; + continue; + } + if (priority_i > new_notification->priority) { + insert_point = i; + } + } + + if (insert_point != first_free) { + // there is a free slot, move everything up one place + if (first_free != -1) { + for (uint8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i > insert_point; i--) { + status->queued_priorities[i] = status->queued_priorities[i - 1]; + status->queued_sequences[i] = status->queued_sequences[i - 1]; + } + if (status->active_sequence_num >= insert_point) { + status->active_sequence_num++; + } + } else { + // no free space, discard lowest priority notification and move everything down + for (uint8_t i = 0; i < insert_point; i++) { + status->queued_priorities[i] = status->queued_priorities[i + 1]; + status->queued_sequences[i] = status->queued_sequences[i + 1]; + } + if (status->active_sequence_num <= insert_point) { + status->active_sequence_num--; + } + } + } + + status->queued_priorities[insert_point] = new_notification->priority; + status->queued_sequences[insert_point] = new_notification->sequence; + updated_sequence = insert_point; + } + + if (status->active_sequence_num < updated_sequence) { + status->active_sequence_num = updated_sequence; + restart_sequence(status, true); + } + if (updated_sequence == BACKGROUND_SEQUENCE) { + restart_sequence(status, false); + } +} + +static bool pop_queued_sequence(NotifierLedStatus_t *status) +{ + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // start the lower priority item + status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND; + status->active_sequence_num--; + return true; + } + // background sequence was completed + return false; +} + +/** + * advance current sequence pointers for next step + */ +static void advance_sequence(NotifierLedStatus_t *status) +{ + LedSequence_t *activeSequence = + status->active_sequence_num == BACKGROUND_SEQUENCE ? + &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + + uint8_t step = status->next_sequence_step; + LedStep_t *currentStep = &activeSequence->steps[step]; + + // Next step will be the OFF phase, so just update the time and + if (status->step_phase_on) { + // next will be the off phase + status->next_run_time += currentStep->time_on; + status->step_phase_on = false; + // check if off phase should be skipped + if (currentStep->time_off != 0) { + return; + } + } + + // next step is ON phase. check whether to repeat current step or move to next one + status->next_run_time += currentStep->time_off; + status->step_phase_on = true; + + if (status->next_step_rep + 1 < currentStep->repeats) { + // setup next repetition + status->next_step_rep++; + return; + } + + // move to next step + LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0; + + // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences + if (NOTIFY_IS_NULL_STEP(nextStep)) { + if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) { + status->next_sequence_rep++; + // restart the sequence + restart_sequence(status, false); + return; + } + if (status->active_sequence_num != BACKGROUND_SEQUENCE) { + // no repeat, pop enqueued or background sequences + pop_queued_sequence(status); + restart_sequence(status, false); + status->next_sequence_rep = 0; + } else { + status->running = false; + } + } else { + status->next_step_rep = 0; + status->next_sequence_step++; + } +} + +/** + * run a led set + */ +static void run_led(NotifierLedStatus_t *status) +{ + uint32_t currentTime = GET_CURRENT_MILLIS; + + if (!status->running || currentTime < status->next_run_time) { + return; + } + status->next_run_time = currentTime; + uint8_t step = status->next_sequence_step; + + LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ? + &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; + if (status->step_phase_on) { + PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true); + } else { + PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); + } + advance_sequence(status); +} + +void LedNotificationExtLedsRun() +{ + // handle incoming sequences + if (!led_status_initialized) { + InitExtLed(); + led_status_initialized = true; + } + static ExtLedNotification_t *newNotification; + newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true); + if (newNotification) { + push_queued_sequence(newNotification, &led_status[0]); + } + + // Run Leds + for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) { + run_led(&led_status[i]); + } +} diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index c02152e02..cf704d4d5 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -31,40 +31,10 @@ #include #include -#ifdef PIOS_INCLUDE_WS2811 -#include -#endif -// Private defines -// Maximum number of notifications enqueued when a higher priority notification is added -#define MAX_BACKGROUND_NOTIFICATIONS 3 -#define MAX_HANDLED_LED 1 -#define BACKGROUND_SEQUENCE -1 -#define RESET_STEP -1 -#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS) +#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS) // Private data types definition -// this is the status for a single LED notification led set -typedef struct { - int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; - LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; - LedSequence_t background_sequence; - uint32_t next_run_time; - uint32_t sequence_starting_time; - - int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE - bool running; // is this led running? - bool step_phase_on; // true = step on phase, false = step off phase - uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) - uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) - uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) - uint8_t led_set; // target led set -} NotifierLedStatus_t; - -#ifdef PIOS_INCLUDE_WS2811 -static bool led_status_initialized = false; -#endif - #ifdef PIOS_LED_ALARM #define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) #define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM) @@ -173,7 +143,6 @@ static bool handleNotifications(pios_notify_notification runningNotification, ui static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern); static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern); -static void HandleExtLeds(); void NotificationUpdateStatus() { @@ -202,8 +171,8 @@ void NotificationOnboardLedsRun() STATUS_LENGHT } status; - HandleExtLeds(); const uint32_t current_timestamp = GET_CURRENT_MILLIS; + if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) { return; } @@ -334,212 +303,3 @@ static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern) *b_pattern = BLINK_B_HEARTBEAT_PATTERN; *r_pattern = BLINK_R_HEARTBEAT_PATTERN; } - -#ifdef PIOS_INCLUDE_WS2811 - -NotifierLedStatus_t led_status[MAX_HANDLED_LED]; - -static void InitExtLed() -{ - memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); - const uint32_t now = GET_CURRENT_MILLIS; - for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { - led_status[l].led_set = l; - led_status[l].next_run_time = now + 500; // start within half a second - for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { - led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; - } - } -} - -/** - * restart current sequence - */ -static void restart_sequence(NotifierLedStatus_t *status, bool immediate) -{ - status->next_sequence_step = 0; - status->next_step_rep = 0; - status->step_phase_on = true; - status->running = true; - if (immediate) { - uint32_t currentTime = GET_CURRENT_MILLIS; - status->next_run_time = currentTime; - } - status->sequence_starting_time = status->next_run_time; -} - -/** - * modify background sequence or enqueue a new sequence to play - */ -static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status) -{ - int8_t updated_sequence = BACKGROUND_SEQUENCE; - - if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) { - status->background_sequence = new_notification->sequence; - } else { - // a notification with priority higher than background. - // try to enqueue it - int8_t insert_point = -1; - int8_t first_free = -1; - for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; i--) { - const int8_t priority_i = status->queued_priorities[i]; - if (priority_i == NOTIFY_PRIORITY_BACKGROUND) { - first_free = i; - insert_point = i; - continue; - } - if (priority_i > new_notification->priority) { - insert_point = i; - } - } - - if (insert_point != first_free) { - // there is a free slot, move everything up one place - if (first_free != -1) { - for (uint8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i > insert_point; i--) { - status->queued_priorities[i] = status->queued_priorities[i - 1]; - status->queued_sequences[i] = status->queued_sequences[i - 1]; - } - if (status->active_sequence_num >= insert_point) { - status->active_sequence_num++; - } - } else { - // no free space, discard lowest priority notification and move everything down - for (uint8_t i = 0; i < insert_point; i++) { - status->queued_priorities[i] = status->queued_priorities[i + 1]; - status->queued_sequences[i] = status->queued_sequences[i + 1]; - } - if (status->active_sequence_num <= insert_point) { - status->active_sequence_num--; - } - } - } - - status->queued_priorities[insert_point] = new_notification->priority; - status->queued_sequences[insert_point] = new_notification->sequence; - updated_sequence = insert_point; - } - - if (status->active_sequence_num < updated_sequence) { - status->active_sequence_num = updated_sequence; - restart_sequence(status, true); - } - if (updated_sequence == BACKGROUND_SEQUENCE) { - restart_sequence(status, false); - } -} - -static bool pop_queued_sequence(NotifierLedStatus_t *status) -{ - if (status->active_sequence_num != BACKGROUND_SEQUENCE) { - // start the lower priority item - status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND; - status->active_sequence_num--; - return true; - } - // background sequence was completed - return false; -} - -/** - * advance current sequence pointers for next step - */ -static void advance_sequence(NotifierLedStatus_t *status) -{ - LedSequence_t *activeSequence = - status->active_sequence_num == BACKGROUND_SEQUENCE ? - &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; - - uint8_t step = status->next_sequence_step; - LedStep_t *currentStep = &activeSequence->steps[step]; - - // Next step will be the OFF phase, so just update the time and - if (status->step_phase_on) { - // next will be the off phase - status->next_run_time += currentStep->time_on; - status->step_phase_on = false; - // check if off phase should be skipped - if (currentStep->time_off != 0) { - return; - } - } - - // next step is ON phase. check whether to repeat current step or move to next one - status->next_run_time += currentStep->time_off; - status->step_phase_on = true; - - if (status->next_step_rep + 1 < currentStep->repeats) { - // setup next repetition - status->next_step_rep++; - return; - } - - // move to next step - LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0; - - // next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences - if (NOTIFY_IS_NULL_STEP(nextStep)) { - if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) { - status->next_sequence_rep++; - // restart the sequence - restart_sequence(status, false); - return; - } - if (status->active_sequence_num != BACKGROUND_SEQUENCE) { - // no repeat, pop enqueued or background sequences - pop_queued_sequence(status); - restart_sequence(status, false); - } else { - status->running = false; - } - } else { - status->next_step_rep = 0; - status->next_sequence_step++; - } -} - -/** - * run a led set - */ -static void run_led(NotifierLedStatus_t *status) -{ - uint32_t currentTime = GET_CURRENT_MILLIS; - - if (!status->running || currentTime < status->next_run_time) { - return; - } - status->next_run_time = currentTime; - uint8_t step = status->next_sequence_step; - - LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ? - &status->background_sequence : &status->queued_sequences[status->active_sequence_num]; - if (status->step_phase_on) { - PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true); - } else { - PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); - } - advance_sequence(status); -} - -void HandleExtLeds() -{ - // handle incoming sequences - if (!led_status_initialized) { - InitExtLed(); - led_status_initialized = true; - } - static ExtLedNotification_t *newNotification; - newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true); - if (newNotification) { - push_queued_sequence(newNotification, &led_status[0]); - } - - // Run Leds - for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) { - run_led(&led_status[i]); - } -} -#else /* ifdef PIOS_INCLUDE_WS2811 */ -void HandleExtLeds() {} -#endif /* ifdef PIOS_INCLUDE_WS2811 */ diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index aa6a8f54d..e37168061 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -42,7 +42,11 @@ #include // private includes #include "inc/systemmod.h" -#include "notification.h" + +#include +#ifdef PIOS_INCLUDE_WS2811 +#include +#endif // UAVOs #include @@ -55,6 +59,7 @@ #include #include #include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -633,6 +638,9 @@ static void updateSystemAlarms() void vApplicationIdleHook(void) { NotificationOnboardLedsRun(); +#ifdef PIOS_INCLUDE_WS2811 + LedNotificationExtLedsRun(); +#endif } /** * Called by the RTOS when a stack overflow is detected. diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index 608d2b6cf..29cf57378 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -87,6 +87,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/lednotification.c ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index cb99372f1..e4220ade0 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -88,6 +88,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/plans.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c + SRC += $(FLIGHTLIB)/lednotification.c ## UAVObjects include ./UAVObjects.inc From 376aa0052f212412b5ad2c83e24c97868767fa00 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 13 Aug 2014 15:18:17 +0200 Subject: [PATCH 124/718] OP-1379 - Initial Notify module. Supports: 1) predefined seqences associated with each flightmode. 2) fixed table with alarm index and assciated led warn/error sequences/repetition rate. --- flight/modules/Notify/inc/notify.h | 33 ++ flight/modules/Notify/inc/sequences.h | 417 ++++++++++++++++++ flight/modules/Notify/notify.c | 142 ++++++ .../boards/revolution/firmware/Makefile | 1 + 4 files changed, 593 insertions(+) create mode 100644 flight/modules/Notify/inc/notify.h create mode 100644 flight/modules/Notify/inc/sequences.h create mode 100644 flight/modules/Notify/notify.c diff --git a/flight/modules/Notify/inc/notify.h b/flight/modules/Notify/inc/notify.h new file mode 100644 index 000000000..1b779bf27 --- /dev/null +++ b/flight/modules/Notify/inc/notify.h @@ -0,0 +1,33 @@ +/** + ****************************************************************************** + * + * @file notify.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify module, show events and status on external led. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 NOTIFY_H_ +#define NOTIFY_H_ + + +int32_t NotifyInitialize(void); + + +#endif /* NOTIFY_H_ */ diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h new file mode 100644 index 000000000..ebb9bafb6 --- /dev/null +++ b/flight/modules/Notify/inc/sequences.h @@ -0,0 +1,417 @@ +/** + ****************************************************************************** + * + * @file sequences.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify module, sequences configuration. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 SEQUENCES_H_ +#define SEQUENCES_H_ +#include +#include +#include +#include +#include + +typedef enum { + NOTIFY_SEQUENCE_ARMED_FM_MANUAL = 0, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1 = 1, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2 = 2, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3 = 3, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4 = 4, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5 = 5, + NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6 = 6, + NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE = 7, + NOTIFY_SEQUENCE_ARMED_FM_GPS = 8, + NOTIFY_SEQUENCE_ARMED_FM_RTH = 9, + NOTIFY_SEQUENCE_ARMED_FM_LAND = 10, + NOTIFY_SEQUENCE_ARMED_FM_AUTO = 11, + NOTIFY_SEQUENCE_ALM_WARN_GPS = 12, + NOTIFY_SEQUENCE_ALM_ERROR_GPS = 13, + NOTIFY_SEQUENCE_ALM_WARN_BATTERY = 14, + NOTIFY_SEQUENCE_ALM_ERROR_BATTERY = 15, + NOTIFY_SEQUENCE_ALM_MAG = 16, + NOTIFY_SEQUENCE_ALM_CONFIG = 17, + NOTIFY_SEQUENCE_DISARMED = 18, +} NotifySequences; + +typedef struct { + uint32_t timeBetweenNotifications; + uint8_t alarmIndex; + uint8_t warnNotification; + uint8_t errorNotification; +} AlarmDefinition_t; + +// consts +const LedSequence_t notifications[] = { + [NOTIFY_SEQUENCE_DISARMED] = { + .repeats = -1, + .steps = { + { + .time_off = 1000, + .time_on = 1000, + .color = COLOR_BLUE, + .repeats = 1, + }, + { + .time_off = 1000, + .time_on = 1000, + .color = COLOR_LIME, + .repeats = 1, + }, + { + .time_off = 1000, + .time_on = 1000, + .color = COLOR_RED, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_MANUAL] = { + .repeats = -1, + .steps = { + { + .time_off = 900, + .time_on = 100, + .color = COLOR_YELLOW, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1] = { + .repeats = -1, + .steps = { + { + .time_off = 900, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + { + .time_off = 700, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 2, + }, + { + .time_off = 500, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4] = { + .repeats = -1, + .steps = { + { + .time_off = 900, + .time_on = 100, + .color = COLOR_PURPLE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_PURPLE, + .repeats = 1, + }, + { + .time_off = 700, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_PURPLE, + .repeats = 1, + }, + { + .time_off = 100, + .time_on = 100, + .color = COLOR_PURPLE, + .repeats = 1, + }, + { + .time_off = 500, + .time_on = 100, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE] = { + .repeats = -1, + .steps = { + { + .time_off = 800, + .time_on = 200, + .color = COLOR_BLUE, + .repeats = 1, + }, + }, + }, + + [NOTIFY_SEQUENCE_ARMED_FM_GPS] = { + .repeats = -1, + .steps = { + { + .time_off = 800, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + }, + }, + + [NOTIFY_SEQUENCE_ARMED_FM_RTH] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + { + .time_off = 500, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_LAND] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + { + .time_off = 100, + .time_on = 200, + .color = COLOR_BLUE, + .repeats = 1, + }, + { + .time_off = 100, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + { + .time_off = 100, + .time_on = 200, + .color = COLOR_BLUE, + .repeats = 1, + }, + { + .time_off = 100, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ARMED_FM_AUTO] = { + .repeats = -1, + .steps = { + { + .time_off = 100, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 2, + }, + { + .time_off = 500, + .time_on = 200, + .color = COLOR_GREEN, + .repeats = 1, + }, + }, + }, + + [NOTIFY_SEQUENCE_ALM_WARN_GPS] = { + .repeats = 2, + .steps = { + { + .time_off = 300, + .time_on = 300, + .color = COLOR_ORANGE, + .repeats = 2, + }, + { + .time_off = 300, + .time_on = 300, + .color = COLOR_YELLOW, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ALM_ERROR_GPS] = { + .repeats = 2, + .steps = { + { + .time_off = 300, + .time_on = 300, + .color = COLOR_RED, + .repeats = 2, + }, + { + .time_off = 300, + .time_on = 300, + .color = COLOR_YELLOW, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ALM_WARN_BATTERY] = { + .repeats = 1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_ORANGE, + .repeats = 10, + }, + }, + }, + [NOTIFY_SEQUENCE_ALM_ERROR_BATTERY] = { + .repeats = 1, + .steps = { + { + .time_off = 100, + .time_on = 100, + .color = COLOR_RED, + .repeats = 10, + }, + }, + }, + [NOTIFY_SEQUENCE_ALM_MAG] = { + .repeats = 1, + .steps = { + { + .time_off = 300, + .time_on = 300, + .color = COLOR_RED, + .repeats = 2, + }, + { + .time_off = 300, + .time_on = 300, + .color = COLOR_GREEN, + .repeats = 1, + }, + }, + }, + [NOTIFY_SEQUENCE_ALM_CONFIG] = { + .repeats = -1, + .steps = { + { + .time_off = 500, + .time_on = 100, + .color = COLOR_RED, + .repeats = 1, + }, + }, + }, +}; + +const LedSequence_t *flightModeMap[] = { + [FLIGHTSTATUS_FLIGHTMODE_MANUAL] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED2] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED3] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED4] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED5] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5], + [FLIGHTSTATUS_FLIGHTMODE_STABILIZED6] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6], + [FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_AUTO], + [FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE], + [FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_RTH], + [FLIGHTSTATUS_FLIGHTMODE_LAND] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND], + [FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], +}; + +const AlarmDefinition_t alarmsMap[] = { + { + .timeBetweenNotifications = 10000, + .alarmIndex = SYSTEMALARMS_ALARM_GPS, + .warnNotification = NOTIFY_SEQUENCE_ALM_WARN_GPS, + .errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_GPS, + }, + { + .timeBetweenNotifications = 15000, + .alarmIndex = SYSTEMALARMS_ALARM_MAGNETOMETER, + .warnNotification = NOTIFY_SEQUENCE_ALM_MAG, + .errorNotification = NOTIFY_SEQUENCE_ALM_MAG, + }, + { + .timeBetweenNotifications = 15000, + .alarmIndex = SYSTEMALARMS_ALARM_BATTERY, + .warnNotification = NOTIFY_SEQUENCE_ALM_WARN_BATTERY, + .errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_BATTERY, + }, +}; +const uint8_t alarmsMapSize = NELEMENTS(alarmsMap); + +#endif /* SEQUENCES_H_ */ diff --git a/flight/modules/Notify/notify.c b/flight/modules/Notify/notify.c new file mode 100644 index 000000000..a944690cb --- /dev/null +++ b/flight/modules/Notify/notify.c @@ -0,0 +1,142 @@ +/** + ****************************************************************************** + * + * @file notify.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify module, show events and status on external led. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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 + */ + +/** + * Input objects: ExampleObject1, ExampleSettings + * Output object: ExampleObject2 + * + * This module executes in response to ExampleObject1 updates. When the + * module is triggered it will update the data of ExampleObject2. + * + * No threads are used in this example. + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ +#include "openpilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "inc/notify.h" +#include "inc/sequences.h" +#include + +#define SAMPLE_PERIOD_MS 250 +// private types +typedef struct { + uint32_t lastAlarmTime; + uint8_t lastAlarm; +} AlarmStatus_t; +// function declarations +static void updatedCb(UAVObjEvent *ev); +static void onTimerCb(UAVObjEvent *ev); +static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, + uint8_t warn_sequence, uint8_t error_sequence, + uint32_t timeBetweenNotifications); +static AlarmStatus_t *alarmStatus; +int32_t NotifyInitialize(void) +{ + alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize); + for (uint8_t i = 0; i < alarmsMapSize; i++) { + alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK; + alarmStatus[i].lastAlarmTime = 0; + } + + FlightStatusConnectCallback(&updatedCb); + updatedCb(0); + static UAVObjEvent ev; + memset(&ev, 0, sizeof(UAVObjEvent)); + EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + return 0; +} +MODULE_INITCALL(NotifyInitialize, 0); + + +void updatedCb(UAVObjEvent *ev) +{ + if (!ev || ev->obj == FlightStatusHandle()) { + static uint8_t last_armed = 0xff; + static uint8_t last_flightmode = 0xff; + uint8_t armed; + uint8_t flightmode; + FlightStatusArmedGet(&armed); + FlightStatusFlightModeGet(&flightmode); + if (last_armed != armed || (armed && flightmode != last_flightmode)) { + if (armed) { + PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND); + } else { + PIOS_NOTIFICATION_Default_Ext_Led_Play(¬ifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND); + } + } + last_armed = armed; + last_flightmode = flightmode; + } +} + +void onTimerCb(__attribute__((unused)) UAVObjEvent *ev) +{ + static SystemAlarmsAlarmData alarms; + + SystemAlarmsAlarmGet(&alarms); + for (uint8_t i = 0; i < alarmsMapSize; i++) { + uint8_t alarm = ((uint8_t *)&alarms)[alarmsMap[i].alarmIndex]; + checkAlarm(alarm, + &alarmStatus[i].lastAlarm, + &alarmStatus[i].lastAlarmTime, + alarmsMap[i].warnNotification, + alarmsMap[i].errorNotification, + alarmsMap[i].timeBetweenNotifications); + } +} + +void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, uint8_t warn_sequence, uint8_t error_sequence, uint32_t timeBetweenNotifications) +{ + if (alarm > SYSTEMALARMS_ALARM_OK) { + uint32_t current_time = PIOS_DELAY_GetuS(); + if (*last_alarm < alarm || *last_alm_time + timeBetweenNotifications < current_time) { + uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence; + PIOS_NOTIFICATION_Default_Ext_Led_Play( + ¬ifications[sequence], + alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL); + *last_alarm = alarm; + *last_alm_time = current_time; + } + } else { + *last_alarm = SYSTEMALARMS_ALARM_OK; + } +} diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index e4220ade0..b544ab4dd 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -50,6 +50,7 @@ MODULES += FixedWingPathFollower MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry +MODULES += Notify OPTMODULES += ComUsbBridge From 30df5de3323c72efe8092f554fb6eb8df74b5c28 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 13 Aug 2014 15:54:52 +0200 Subject: [PATCH 125/718] uncrustify --- flight/pios/inc/pios_flash_jedec_catalog.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index f35473538..eae55063d 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -76,8 +76,8 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, .expect_memorytype = 0xBA, .expect_capacity = 0x20, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, @@ -85,8 +85,8 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .expect_manufacturer = JEDEC_MANUFACTURER_NUMORIX, .expect_memorytype = 0xBA, .expect_capacity = 0x19, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, @@ -94,8 +94,8 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .expect_manufacturer = JEDEC_MANUFACTURER_MICRON, .expect_memorytype = 0xBA, .expect_capacity = 0x18, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, .fast_read = 0x0B, .fast_read_dummy_bytes = 1, }, From 7cd4164358d0992fc8a4b84e559e1762a483c993 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Thu, 14 Aug 2014 06:25:18 +1000 Subject: [PATCH 126/718] following op-11400 qt 531 upgrade branch. Recommit mac tools.mk changes --- make/tools.mk | 88 +++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 85 insertions(+), 3 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index af1e76f16..14e874e81 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -76,7 +76,12 @@ ifeq ($(UNAME), Linux) else ifeq ($(UNAME), Darwin) ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2 ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2/+md5 - QT_SDK_URL := "Please install native Qt 5.3.x SDK using package manager" + QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-mac-x64-clang-5.3.1.dmg + QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-mac-x64-clang-5.3.1.dmg.md5 + QT_SDK_ARCH := clang_64 + QT_SDK_MAINTENANCE_TOOL := /Volumes/qt-opensource-mac-x64-clang-5.3.1/qt-opensource-mac-x64-clang-5.3.1.app/Contents/MacOS/qt-opensource-mac-x64-clang-5.3.1 + QT_SDK_MOUNT_DIR := /Volumes/qt-opensource-mac-x64-clang-5.3.1 + QT_SDK_INSTALLER_DAT := /Volumes/qt-opensource-mac-x64-clang-5.3.1/qt-opensource-mac-x64-clang-5.3.1.app/Contents/Resources/installer.dat UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz DOXYGEN_URL := http://wiki.openpilot.org/download/attachments/18612236/doxygen-1.8.3.1.src.tar.gz else ifeq ($(UNAME), Windows) @@ -467,6 +472,79 @@ qt_sdk_distclean: endef + +############################## +# +# Mac QT install template +# $(1) = tool temp extract/build directory +# $(2) = tool install directory +# $(3) = tool distribution URL +# $(4) = tool distribution .md5 URL +# $(5) = tool distribution file +# $(6) = QT architecture +# $(7) = optional extra build recipes template +# $(8) = optional extra clean recipes template +# +############################## + +define MAC_QT_INSTALL_TEMPLATE + +.PHONY: $(addprefix qt_sdk_, install clean distclean) + +qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) + $(V1) if ! $(SEVENZIP) >/dev/null 2>&1; then \ + $(ECHO) $(MSG_NOTICE) "Please install the p7zip for your distribution. i.e.: sudo brew install p7zip." && \ + exit 1; \ + fi + $(call DOWNLOAD_TEMPLATE,$(3),$(5),"$(4)") +# Mount .dmg file + $(V1) hdiutil attach -nobrowse $(DL_DIR)/$(5) +# Explode .dmg file into install packages + @$(ECHO) $(MSG_EXTRACTING) $$(call toprel, $(1)) + $(V1) $(MKDIR) -p $$(call toprel, $(dir $(1))) + $(V1) $(QT_SDK_MAINTENANCE_TOOL) --dump-binary-data -i $(QT_SDK_INSTALLER_DAT) -o $(1) +# Extract packages under tool directory + $(V1) $(MKDIR) -p $$(call toprel, $(dir $(2))) + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_essentials.7z" | grep -v Extracting +# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6).essentials/5.3.1icu_path_patcher.sh.7z" | grep -v Extracting + $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_addons.7z" | grep -v Extracting +# go to OpenPilot/tools/5.3.1/gcc_64 and call patcher.sh + @$(ECHO) + @$(ECHO) "Running patcher in" $$(call toprel, $(QT_SDK_PREFIX)) + $(V1) $(CD) $(QT_SDK_PREFIX) +# $(V1) "$(QT_SDK_PREFIX)/patcher.sh" $(QT_SDK_PREFIX) +# call qmake patcher + @$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX)) + $(V1) $(QT_SDK_MAINTENANCE_TOOL) --runoperation QtPatch linux $(QT_SDK_PREFIX) qt5 + +#Unmount the .dmg file + $(V1) hdiutil detach $(QT_SDK_MOUNT_DIR) + +# Execute post build templates + $(7) + +# Clean up temporary files + @$(ECHO) $(MSG_CLEANING) $$(call toprel, $(1)) + $(V1) [ ! -d "$(1)" ] || $(RM) -rf "$(1)" + +qt_sdk_clean: + @$(ECHO) $(MSG_CLEANING) $$(call toprel, $(1)) + $(V1) [ ! -d "$(1)" ] || $(RM) -rf "$(1)" + @$(ECHO) $(MSG_CLEANING) $$(call toprel, "$(2)") + $(V1) [ ! -d "$(2)" ] || $(RM) -rf "$(2)" + + $(8) + +qt_sdk_distclean: + @$(ECHO) $(MSG_DISTCLEANING) $$(call toprel, $(DL_DIR)/$(5)) + $(V1) [ ! -f "$(DL_DIR)/$(5)" ] || $(RM) "$(DL_DIR)/$(5)" + $(V1) [ ! -f "$(DL_DIR)/$(5).md5" ] || $(RM) "$(DL_DIR)/$(5).md5" + +endef + ############################## # # ARM SDK @@ -504,8 +582,6 @@ endef # # Qt SDK # -# Mac OS X: user should install native Qt SDK package -# ############################## ifeq ($(UNAME), Windows) @@ -529,6 +605,12 @@ QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)" QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD $(eval $(call LINUX_QT_INSTALL_TEMPLATE,$(QT_BUILD_DIR),$(QT_SDK_DIR),$(QT_SDK_URL),$(QT_SDK_MD5_URL),$(notdir $(QT_SDK_URL)),$(QT_SDK_ARCH))) +else ifeq ($(UNAME), Darwin) + +QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)" +QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD + $(eval $(call MAC_QT_INSTALL_TEMPLATE,$(QT_BUILD_DIR),$(QT_SDK_DIR),$(QT_SDK_URL),$(QT_SDK_MD5_URL),$(notdir $(QT_SDK_URL)),$(QT_SDK_ARCH))) + else QT_SDK_PREFIX := $(QT_SDK_DIR) From eefe40c6a6deec64d16656e9a5ef53aedc267a28 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Thu, 14 Aug 2014 06:56:31 +1000 Subject: [PATCH 127/718] changed qt patch to mac target --- make/tools.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/make/tools.mk b/make/tools.mk index 14e874e81..913b90901 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -518,7 +518,7 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) # $(V1) "$(QT_SDK_PREFIX)/patcher.sh" $(QT_SDK_PREFIX) # call qmake patcher @$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX)) - $(V1) $(QT_SDK_MAINTENANCE_TOOL) --runoperation QtPatch linux $(QT_SDK_PREFIX) qt5 + $(V1) $(QT_SDK_MAINTENANCE_TOOL) --runoperation QtPatch mac $(QT_SDK_PREFIX) qt5 #Unmount the .dmg file $(V1) hdiutil detach $(QT_SDK_MOUNT_DIR) From 53fd76911e8e61b8f1cd55562091bb53adbb2421 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 4 Aug 2014 20:40:02 +0200 Subject: [PATCH 128/718] OP-1433 Group the PID options by Roll, Pitch, Roll+Pitch, and Yaw. --- shared/uavobjectdefinition/txpidsettings.xml | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index e85774e9b..c1bd56740 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -11,15 +11,14 @@ From ab43fbe7a861e6d1cbfe4adc9596e56724abbb93 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Fri, 15 Aug 2014 21:17:01 +1000 Subject: [PATCH 129/718] Fix missing lib --- ground/openpilotgcs/copydata.pro | 1 + 1 file changed, 1 insertion(+) diff --git a/ground/openpilotgcs/copydata.pro b/ground/openpilotgcs/copydata.pro index 58a46dddf..2e3f56306 100644 --- a/ground/openpilotgcs/copydata.pro +++ b/ground/openpilotgcs/copydata.pro @@ -30,6 +30,7 @@ GCS_LIBRARY_PATH libQt5Qml.so.5 \ libQt5DBus.so.5 \ libQt5QuickParticles.so.5 \ + libqgsttools_p.so.1 data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline() for(lib, QT_LIBS) { From 731153043a0ad4a4b40a09f08f7c02be58c0d422 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Fri, 15 Aug 2014 21:23:49 +1000 Subject: [PATCH 130/718] Old logo file --- .../src/plugins/coreplugin/coreplugin.pro | 1 - .../coreplugin/images/qtcreator_logo_32.png | Bin 1410 -> 0 bytes 2 files changed, 1 deletion(-) delete mode 100644 ground/openpilotgcs/src/plugins/coreplugin/images/qtcreator_logo_32.png diff --git a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro index 5c4269c69..d329bdbf8 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro +++ b/ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro @@ -143,7 +143,6 @@ RESOURCES += core.qrc \ unix:!macx { images.files = images/openpilot_logo_*.png - images.files = images/qtcreator_logo_*.png images.path = /share/pixmaps INSTALLS += images } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/qtcreator_logo_32.png b/ground/openpilotgcs/src/plugins/coreplugin/images/qtcreator_logo_32.png deleted file mode 100644 index 274fb8342f3114a983a2827e8949d2f529c2448f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1410 zcmV-|1%3L7P)8P6s4LV=@PeA=$Erahrzed_WiuHX;~8 zO5AugunscX($T_L%cG?ap!J>${VCA$SXp+G)BoQ4-`n%u^Zr>5hXWoZ`4kluLGgd5 zkCSnJN*Y`EbVPvb`+o>439Dwt!%{_d>uLgEv3G=_@B>6R%oDyCwax%*bB-Sr7y_U+ z$jw{38W)-ajDY%cNfY1?o4slpji2E+oEKc`02>}L`FMJvA0CJLYc|-lnFaZ;?1Fb< zj(~a62(tdap!~+^RRiEw17?d2YO1VoXkRikeXfF!6OMyF_X!a3*F%kI9{h6F!E@iPr*j1I+Oxap*lq`SKfZ8vHDDZb zKmxI4!|Ad2z{o6 z?qt?(Y31~Z!WtCU=~%!C1W^bZMt)W^`@Y=zD;!Bb=baM{8|)x{YVO*&)jAEp-@yQ+ zzG8MjMfVBn7o*nuP$K&XOje_J8WrNfrIwpt-O0-UqgAo6U-< z5xK|1fkMBB6NWci46{OfLmz4)atpxK4Crj0s9!hct^73$0IZ8xJUhq5&xV=BV)sV) ztAt|~NaOL?pu0QI)&l?O{4mOzV|R?02c&eu#|jGzH46ZQiUsuPOgKofp(#v)+PdK- zCOe|3T>S;du5-8bpT&CH$H%A2WHQx}Dc7yOF%J}<{EeCC$9)YoSBK%{?Oa+HkScDU zfG0&P=n`){-lS_2TUrsBL5wpi5Aj%^r3KQ?yn@pp9NR%oOS+9r*lC1&a9xIpq{j}w7j^u zI2arp9IdUbZ6+qL`nZS6f!DJEe!8fFs@iZU*?W*?JES_*1vu069kX{w)-;IHhA*5; z=t0GV&tOL9a=E3kv9YOf%a1>l>@$C!2gV*wBr z6yolPjqqW@64Ht_Z|r`nP9l-WGBPq!6A}_&YHA9ELLq208q~nQ=);RPB(7nHy@7J1 zgI2vQq)3VRkx)?muIo0;9GTdd5IvRX0>J82BSrajxCs6Lw(9i6Fn{2%Z*0kQ6e}xDk{nuZlqe8sm%of0ZCdG zP^;C@*Vk8p-oGLM9?1F8a*Nzv&l0e5h^I3zCnqOiU|@in0fpOaHt6c=apB96_GQZnqB;4n@_Ys)K`r>8+7b6LPuS($>~?klfz1pd&gU`CvCFysoU+iNp*I z4FR9eM?h3KhFxc8r-eA)-iC&T=JfRR<$V{dlt}nB5HF{Ng5-LPb$W9Wc4`73ud=eT zk=)*~><^Qb5-Bd1+S}XnI2;aJuh*+dRNN&IaK5ap>>3JAN=kBj?tcLW09$G$2mF$$ Q3;+NC07*qoM6N<$f&@OL5dZ)H From b562fcb02ef50d88ddd4ef419a6428170a82c126 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 17:56:57 +0200 Subject: [PATCH 131/718] OP-1156 fix path logic to not deviate from correct altitude too much --- flight/libraries/inc/paths.h | 6 +- flight/libraries/paths.c | 182 ++++++++---------- flight/libraries/plans.c | 19 +- flight/modules/PathFollower/pathfollower.c | 85 +++----- flight/modules/PathPlanner/pathplanner.c | 10 +- .../flightmodesettings.xml | 2 - 6 files changed, 119 insertions(+), 185 deletions(-) diff --git a/flight/libraries/inc/paths.h b/flight/libraries/inc/paths.h index 4cecbe845..053b9b474 100644 --- a/flight/libraries/inc/paths.h +++ b/flight/libraries/inc/paths.h @@ -30,10 +30,10 @@ struct path_status { float fractional_progress; float error; - float correction_direction[3]; - float path_direction[3]; + float path_vector[3]; + float correction_vector[3]; }; -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode); +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status); #endif diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 6c80d0a9d..3bcd0a03d 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -28,54 +28,51 @@ #include #include -#include "paths.h" - #include "uavobjectmanager.h" // <--. #include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx, +#include "paths.h" // no direct UAVObject usage allowed in this file // private functions -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode); -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise); +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode); +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise); /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired structure * @param[in] cur_point Current location - * @param[in] mode Path following mode * @param[out] status Structure containing progress along path and deviation */ -void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode) +void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status) { - switch (mode) { + switch (path->Mode) { case PATHDESIRED_MODE_FLYVECTOR: - return path_vector(start_point, end_point, cur_point, status, true); + return path_vector(path, cur_point, status, true); break; case PATHDESIRED_MODE_DRIVEVECTOR: - return path_vector(start_point, end_point, cur_point, status, false); + return path_vector(path, cur_point, status, false); break; case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - return path_circle(start_point, end_point, cur_point, status, 1); + return path_circle(path, cur_point, status, 1); break; case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: - return path_circle(start_point, end_point, cur_point, status, 0); + return path_circle(path, cur_point, status, 0); break; case PATHDESIRED_MODE_FLYENDPOINT: - return path_endpoint(start_point, end_point, cur_point, status, true); + return path_endpoint(path, cur_point, status, true); break; case PATHDESIRED_MODE_DRIVEENDPOINT: default: // use the endpoint as default failsafe if called in unknown modes - return path_endpoint(start_point, end_point, cur_point, status, false); + return path_endpoint(path, cur_point, status, false); break; } @@ -83,127 +80,120 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc /** * @brief Compute progress towards endpoint. Deviation equals distance - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) +static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path[3], diff[3]; + float diff[3]; float dist_path, dist_diff; - // we do not correct in this mode - status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0; - // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to end - diff[0] = end_point[0] - cur_point[0]; - diff[1] = end_point[1] - cur_point[1]; - diff[2] = mode3D ? end_point[2] - cur_point[2] : 0; + diff[0] = path->End.North - cur_point[0]; + diff[1] = path->End.East - cur_point[1]; + diff[2] = mode3D ? path->End.Down - cur_point[2] : 0.0f; dist_diff = vector_lengthf(diff, 3); - dist_path = vector_lengthf(path, 3); + dist_path = vector_lengthf(status->path_vector, 3); if (dist_diff < 1e-6f) { - status->fractional_progress = 1; - status->error = 0; - status->path_direction[0] = status->path_direction[1] = 0; - status->path_direction[2] = 0; + status->fractional_progress = 1; + status->error = 0.0f; + status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f; + // we have no base movement direction in this mode + status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f; + return; } - if (dist_path + 1 > dist_diff) { - status->fractional_progress = 1 - dist_diff / (1 + dist_path); + if (fmaxf(dist_path, 1.0f) > dist_diff) { + status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f); } else { status->fractional_progress = 0; // we don't want fractional_progress to become negative } status->error = dist_diff; - // Compute direction to travel - status->path_direction[0] = diff[0] / dist_diff; - status->path_direction[1] = diff[1] / dist_diff; - status->path_direction[2] = diff[2] / dist_diff; + // Compute correction vector + status->correction_vector[0] = diff[0]; + status->correction_vector[1] = diff[1]; + status->correction_vector[2] = diff[2]; + + // base movement direction in this mode is a constant velocity offset on top of correction in the same direction + status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff; + status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff; + status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff; } /** * @brief Compute progress along path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Ending point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation * @param[in] mode3D set true to include altitude in distance and progress calculation */ -static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D) +static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D) { - float path[3], diff[3]; + float diff[3]; float dist_path; float dot; + float velocity; float track_point[3]; // Distance to go - path[0] = end_point[0] - start_point[0]; - path[1] = end_point[1] - start_point[1]; - path[2] = mode3D ? end_point[2] - start_point[2] : 0; + status->path_vector[0] = path->End.North - path->Start.North; + status->path_vector[1] = path->End.East - path->Start.East; + status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f; // Current progress location relative to start - diff[0] = cur_point[0] - start_point[0]; - diff[1] = cur_point[1] - start_point[1]; - diff[2] = mode3D ? cur_point[2] - start_point[2] : 0; + diff[0] = cur_point[0] - path->Start.North; + diff[1] = cur_point[1] - path->Start.East; + diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f; - dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2]; - dist_path = vector_lengthf(path, 3); + dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2]; + dist_path = vector_lengthf(status->path_vector, 3); if (dist_path > 1e-6f) { // Compute direction to travel & progress - status->path_direction[0] = path[0] / dist_path; - status->path_direction[1] = path[1] / dist_path; - status->path_direction[2] = path[2] / dist_path; status->fractional_progress = dot / (dist_path * dist_path); } else { // Fly towards the endpoint to prevent flying away, // but assume progress=1 either way. - path_endpoint(start_point, end_point, cur_point, status, mode3D); + path_endpoint(path, cur_point, status, mode3D); status->fractional_progress = 1; return; } - // Compute point on track that is closest to our current position. - track_point[0] = status->fractional_progress * path[0] + start_point[0]; - track_point[1] = status->fractional_progress * path[1] + start_point[1]; - track_point[2] = status->fractional_progress * path[2] + start_point[2]; + track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North; + track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East; + track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down; - status->correction_direction[0] = track_point[0] - cur_point[0]; - status->correction_direction[1] = track_point[1] - cur_point[1]; - status->correction_direction[2] = track_point[2] - cur_point[2]; + status->correction_vector[0] = track_point[0] - cur_point[0]; + status->correction_vector[1] = track_point[1] - cur_point[1]; + status->correction_vector[2] = track_point[2] - cur_point[2]; - status->error = vector_lengthf(status->correction_direction, 3); + status->error = vector_lengthf(status->correction_vector, 3); - // Normalize correction_direction but avoid division by zero - if (status->error > 1e-6f) { - status->correction_direction[0] /= status->error; - status->correction_direction[1] /= status->error; - status->correction_direction[2] /= status->error; - } else { - status->correction_direction[0] = 0; - status->correction_direction[1] = 0; - status->correction_direction[2] = 0; - } + // correct movement vector to current velocity + velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity); + status->path_vector[0] = velocity * status->path_vector[0] / dist_path; + status->path_vector[1] = velocity * status->path_vector[1] / dist_path; + status->path_vector[2] = velocity * status->path_vector[2] / dist_path; } /** * @brief Compute progress along circular path and deviation from it - * @param[in] start_point Starting point - * @param[in] end_point Center point + * @param[in] path PathDesired * @param[in] cur_point Current location * @param[out] status Structure containing progress along path and deviation */ -static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise) +static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise) { float radius_north, radius_east, diff_north, diff_east, diff_down; float radius, cradius; @@ -212,30 +202,30 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, float a_diff, a_radius; // Radius - radius_north = end_point[0] - start_point[0]; - radius_east = end_point[1] - start_point[1]; + radius_north = path->End.North - path->Start.North; + radius_east = path->End.East - path->Start.East; // Current location relative to center - diff_north = cur_point[0] - end_point[0]; - diff_east = cur_point[1] - end_point[1]; - diff_down = cur_point[2] - end_point[2]; + diff_north = cur_point[0] - path->End.North; + diff_east = cur_point[1] - path->End.East; + diff_down = cur_point[2] - path->End.Down; radius = sqrtf(squaref(radius_north) + squaref(radius_east)); cradius = sqrtf(squaref(diff_north) + squaref(diff_east)); // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply) - status->path_direction[2] = 0.0f; + status->path_vector[2] = 0.0f; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; if (cradius < 1e-6f) { - // cradius is zero, just fly somewhere and make sure correction is still a normal - status->fractional_progress = 1; - status->correction_direction[0] = 0; - status->correction_direction[1] = 1; - status->path_direction[0] = 1; - status->path_direction[1] = 0; + // cradius is zero, just fly somewhere + status->fractional_progress = 1; + status->correction_vector[0] = 0; + status->correction_vector[1] = 0; + status->path_vector[0] = path->EndingVelocity; + status->path_vector[1] = 0; } else { if (clockwise) { // Compute the normal to the radius clockwise @@ -270,20 +260,18 @@ static void path_circle(float *start_point, float *end_point, float *cur_point, progress = 1.0f - progress; } - status->fractional_progress = progress; + status->fractional_progress = progress; // Compute direction to travel - status->path_direction[0] = normal[0]; - status->path_direction[1] = normal[1]; + status->path_vector[0] = normal[0] * path->EndingVelocity; + status->path_vector[1] = normal[1] * path->EndingVelocity; // Compute direction to correct error - status->correction_direction[0] = status->error * diff_north / cradius; - status->correction_direction[1] = status->error * diff_east / cradius; + status->correction_vector[0] = status->error * diff_north / cradius; + status->correction_vector[1] = status->error * diff_east / cradius; } - status->correction_direction[2] = -diff_down; - - vector_normalizef(status->correction_direction, 3); + status->correction_vector[2] = -diff_down; status->error = fabs(status->error); } diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 4df2a9289..0faac2a2a 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -70,8 +70,6 @@ void plan_setup_positionHold() FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = positionState.North; pathDesired.End.East = positionState.East; @@ -79,7 +77,7 @@ void plan_setup_positionHold() pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = positionState.East; pathDesired.Start.Down = positionState.Down; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -110,8 +108,6 @@ void plan_setup_returnToBase() destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown; FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); pathDesired.End.North = takeoffLocation.North; pathDesired.End.East = takeoffLocation.East; @@ -121,7 +117,7 @@ void plan_setup_returnToBase() pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.Down = destDown; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -131,15 +127,12 @@ void plan_setup_returnToBase() static PiOSDeltatimeConfig landdT; void plan_setup_land() { - float descendspeed; - plan_setup_positionHold(); - FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.StartingVelocity = descendspeed; - pathDesired.EndingVelocity = descendspeed; + pathDesired.StartingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; PathDesiredSet(&pathDesired); PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } @@ -383,8 +376,6 @@ void plan_setup_AutoCruise() FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); - float startingVelocity; - FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); // initialization is flight in direction of the nose. // the velocity is not relevant, as it will be reset by the run function even during first call @@ -404,7 +395,7 @@ void plan_setup_AutoCruise() pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; - pathDesired.StartingVelocity = startingVelocity; + pathDesired.StartingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 6db2c2759..96ee78a9c 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -62,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -432,12 +432,10 @@ static float updatePathBearing() positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, cur, &progress); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0])); + float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); // result is in between 0 and 360 degrees if (yaw < 0.0f) { yaw += 360.0f; @@ -581,40 +579,13 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) positionState.Down + (velocityState.Down * kFF) }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); - - float groundspeed; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_DRIVECIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - case PATHDESIRED_MODE_DRIVECIRCLELEFT: - groundspeed = pathDesired.EndingVelocity; - break; - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_DRIVEENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_DRIVEVECTOR: - default: - groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * - boundf(progress.fractional_progress, 0.0f, 1.0f); - break; - } - // make sure groundspeed is not zero - if (limited && groundspeed < 1e-6f) { - groundspeed = 1e-6f; - } + path_progress(&pathDesired, cur, &progress); // calculate velocity - can be zero if waypoints are too close VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_direction[0]; - velocityDesired.East = progress.path_direction[1]; - velocityDesired.Down = progress.path_direction[2]; - - float error_speed_horizontal = progress.error * kH; - float error_speed_vertical = progress.error * kV; + velocityDesired.North = progress.path_vector[0]; + velocityDesired.East = progress.path_vector[1]; + velocityDesired.Down = progress.path_vector[2]; if (limited && // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) @@ -629,39 +600,27 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) // calculating angles < 90 degrees through dot products - ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) && - ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) { - error_speed_horizontal = 0.0f; - error_speed_vertical = 0.0f; - } - - // calculate correction - can also be zero if correction vector is 0 or no error present - velocityDesired.North += progress.correction_direction[0] * error_speed_horizontal; - velocityDesired.East += progress.correction_direction[1] * error_speed_horizontal; - velocityDesired.Down += progress.correction_direction[2] * error_speed_vertical; - - // scale to correct length - float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down); - if (l > 1e-9f) { - velocityDesired.North *= groundspeed / l; - velocityDesired.East *= groundspeed / l; - velocityDesired.Down *= groundspeed / l; + (vector_lengthf(progress.path_vector, 2) > 1e-6f) && + ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && + ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { + ; } else { - velocityDesired.North = 0.0f; - velocityDesired.East = 0.0f; - velocityDesired.Down = 0.0f; + // calculate correction + velocityDesired.North += progress.correction_vector[0] * kH; + velocityDesired.East += progress.correction_vector[1] * kH; } + velocityDesired.Down += progress.correction_vector[2] * kV; // update pathstatus - pathStatus.error = progress.error; + pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_direction[0]; - pathStatus.path_direction_east = progress.path_direction[1]; - pathStatus.path_direction_down = progress.path_direction[2]; + pathStatus.path_direction_north = progress.path_vector[0]; + pathStatus.path_direction_east = progress.path_vector[1]; + pathStatus.path_direction_down = progress.path_vector[2]; - pathStatus.correction_direction_north = progress.correction_direction[0]; - pathStatus.correction_direction_east = progress.correction_direction[1]; - pathStatus.correction_direction_down = progress.correction_direction[2]; + pathStatus.correction_direction_north = progress.correction_vector[0]; + pathStatus.correction_direction_east = progress.correction_vector[1]; + pathStatus.correction_direction_down = progress.correction_vector[2]; VelocityDesiredSet(&velocityDesired); } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 745a8c17e..32d2afbae 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -525,9 +525,8 @@ static uint8_t conditionLegRemaining() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -550,9 +549,8 @@ static uint8_t conditionBelowError() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), - cast_struct_to_array(pathDesired.End, pathDesired.End.North), - cur, &progress, pathDesired.Mode); + path_progress(&pathDesired, + cur, &progress); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 728fc27c0..5511345ae 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -114,8 +114,6 @@ - - From b1675a2804bafb8bec0d1a94425fd2c99a16a4df Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 19:23:41 +0200 Subject: [PATCH 132/718] OP-1156 added vtolpathfollower velocity limits to attitude control --- flight/modules/PathFollower/pathfollower.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 96ee78a9c..6399fe6b1 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -1044,6 +1044,16 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) ManualControlCommandData manualControlData; ManualControlCommandGet(&manualControlData); + // scale velocity if it is above configured maximum + float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + if (velH > vtolPathFollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; + velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; + } + if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { + velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); + } + // Compute desired north command northError = velocityDesired.North - velocityState.North; i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, From 49c5c7a3d86727edba96e1d5fbda3463949b1e60 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Fri, 15 Aug 2014 20:03:08 +0200 Subject: [PATCH 133/718] OP-1447 Use smaller steps for the output calibration wizard page slider --- .../src/plugins/setupwizard/pages/outputcalibrationpage.ui | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui index 0f0dd1707..94edc6b26 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui @@ -88,10 +88,10 @@ p, li { white-space: pre-wrap; } 1300 - 10 + 1 - 20 + 10 Qt::Horizontal From 915c24c8b6902aecb269c0329fdb01dabe9f4c0c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 15 Aug 2014 20:36:14 +0200 Subject: [PATCH 134/718] OP-1156 changed PID control loops to use generic pid library, readded unnecessary integral and derivative terms to vtolpathfollower --- flight/modules/PathFollower/pathfollower.c | 189 +++++++++--------- .../fixedwingpathfollowersettings.xml | 8 +- .../vtolpathfollowersettings.xml | 14 +- 3 files changed, 101 insertions(+), 110 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 6399fe6b1..69e3cc7aa 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -89,12 +89,13 @@ #define DEADBAND_LOW -0.10f // Private types -struct Integrals { - float vel[3]; - float course; - float speed; - float power; - float airspeed; +struct Globals { + struct pid PIDposH[2]; + struct pid PIDposV; + struct pid PIDvel[3]; + struct pid PIDcourse; + struct pid PIDspeed; + struct pid PIDpower; float poiRadius; float vtolEmergencyFallback; bool vtolEmergencyFallbackSwitch; @@ -104,7 +105,7 @@ struct Integrals { // Private variables static DelayedCallbackInfo *pathFollowerCBInfo; static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS; -static struct Integrals i; +static struct Globals global; static PathStatusData pathStatus; static PathDesiredData pathDesired; static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; @@ -116,7 +117,7 @@ static float indicatedAirspeedStateBias = 0.0f; // Private functions static void pathFollowerTask(void); -static void resetIntegrals(); +static void resetGlobals(); static void SettingsUpdatedCb(UAVObjEvent *ev); static uint8_t updateAutoPilotByFrameType(); static uint8_t updateAutoPilotFixedWing(); @@ -126,7 +127,7 @@ static float updateCourseBearing(); static float updatePathBearing(); static float updatePOIBearing(); static void processPOI(); -static void updatePathVelocity(float kFF, float kH, float kV, bool limited); +static void updatePathVelocity(float kFF, bool limited); static uint8_t updateFixedDesiredAttitude(); static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); static void updateFixedAttitude(); @@ -174,7 +175,7 @@ int32_t PathFollowerInitialize() StabilizationBankInitialize(); // reset integrals - resetIntegrals(); + resetGlobals(); // Create object queue pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES); @@ -198,7 +199,7 @@ static void pathFollowerTask(void) FlightStatusGet(&flightStatus); if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - resetIntegrals(); + resetGlobals(); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -246,7 +247,19 @@ static void pathFollowerTask(void) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings); + + pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit); + pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit); + pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit); + + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); + + pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); + pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + PathDesiredGet(&pathDesired); } @@ -278,18 +291,20 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) /** * reset integrals */ -static void resetIntegrals() +static void resetGlobals() { - i.vel[0] = 0.0f; - i.vel[1] = 0.0f; - i.vel[2] = 0.0f; - i.course = 0.0f; - i.speed = 0.0f; - i.power = 0.0f; - i.airspeed = 0.0f; - i.poiRadius = 0.0f; - i.vtolEmergencyFallback = 0; - i.vtolEmergencyFallbackSwitch = false; + pid_zero(&global.PIDposH[0]); + pid_zero(&global.PIDposH[1]); + pid_zero(&global.PIDposV); + pid_zero(&global.PIDvel[0]); + pid_zero(&global.PIDvel[1]); + pid_zero(&global.PIDvel[2]); + pid_zero(&global.PIDcourse); + pid_zero(&global.PIDspeed); + pid_zero(&global.PIDpower); + global.poiRadius = 0.0f; + global.vtolEmergencyFallback = 0; + global.vtolEmergencyFallbackSwitch = false; } static uint8_t updateAutoPilotByFrameType() @@ -330,7 +345,10 @@ static uint8_t updateAutoPilotByFrameType() */ static uint8_t updateAutoPilotFixedWing() { - updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f); + updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true); return updateFixedDesiredAttitude(); } @@ -341,13 +359,19 @@ static uint8_t updateAutoPilotFixedWing() */ static uint8_t updateAutoPilotVtol() { - if (!i.vtolEmergencyFallbackSwitch) { + if (!global.vtolEmergencyFallbackSwitch) { if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); updateVtolDesiredAttitudeEmergencyFallback(); return 1; } else { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false); + pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false); uint8_t result = 1; bool yaw_attitude = true; float yaw = 0.0f; @@ -370,12 +394,15 @@ static uint8_t updateAutoPilotVtol() } result = updateVtolDesiredAttitude(yaw_attitude, yaw); if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) { - i.vtolEmergencyFallbackSwitch = true; + global.vtolEmergencyFallbackSwitch = true; } return result; } } else { - updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true); + pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f); + pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit); + updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true); updateVtolDesiredAttitudeEmergencyFallback(); return 0; } @@ -454,7 +481,7 @@ static float updatePOIBearing() PositionStateData positionState; PositionStateGet(&positionState); - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; float dLoc[3]; float yaw = 0; /*float elevation = 0;*/ @@ -487,7 +514,7 @@ static float updatePOIBearing() **/ static void processPOI() { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; PositionStateData positionState; @@ -536,14 +563,14 @@ static void processPOI() pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f; } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) { // change radius only when not circling - i.poiRadius = distance + changeRadius; + global.poiRadius = distance + changeRadius; } // don't try to move any closer - if (i.poiRadius >= 3.0f || changeRadius > 0) { + if (global.poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.East = poi.East + (i.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -565,7 +592,7 @@ static void processPOI() /** * Compute desired velocity from the current position and path */ -static void updatePathVelocity(float kFF, float kH, float kV, bool limited) +static void updatePathVelocity(float kFF, bool limited) { PositionStateData positionState; @@ -573,10 +600,12 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) VelocityStateData velocityState; VelocityStateGet(&velocityState); + const float dT = updatePeriod / 1000.0f; + // look ahead kFF seconds - float cur[3] = { positionState.North + (velocityState.North * kFF), - positionState.East + (velocityState.East * kFF), - positionState.Down + (velocityState.Down * kFF) }; + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; struct path_status progress; path_progress(&pathDesired, cur, &progress); @@ -606,10 +635,10 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited) ; } else { // calculate correction - velocityDesired.North += progress.correction_vector[0] * kH; - velocityDesired.East += progress.correction_vector[1] * kH; + velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); + velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); } - velocityDesired.Down += progress.correction_vector[2] * kV; + velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); // update pathstatus pathStatus.error = progress.error; @@ -750,15 +779,6 @@ static uint8_t updateFixedDesiredAttitude() /** * Compute desired thrust command */ - // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedWingPathFollowerSettings.PowerPI.Ki > 0.0f) { - i.power = boundf(i.power + -descentspeedError * dT, - -fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki, - fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki - ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); - } else { - i.power = 0.0f; - } // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = boundf( @@ -768,13 +788,12 @@ static uint8_t updateFixedDesiredAttitude() ); // Compute final thrust response - powerCommand = -descentspeedError * fixedWingPathFollowerSettings.PowerPI.Kp + - i.power * fixedWingPathFollowerSettings.PowerPI.Ki + + powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) + speedErrorToPowerCommandComponent; // Output internal state to telemetry fixedWingPathFollowerStatus.Error.Power = descentspeedError; - fixedWingPathFollowerStatus.ErrorInt.Power = i.power; + fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator; fixedWingPathFollowerStatus.Command.Power = powerCommand; // set thrust @@ -803,17 +822,9 @@ static uint8_t updateFixedDesiredAttitude() result = 0; } - /** * Compute desired pitch command */ - if (fixedWingPathFollowerSettings.SpeedPI.Ki > 0) { - // Integrate with saturation - i.airspeed = boundf(i.airspeed + airspeedError * dT, - -fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki, - fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki); - } - // Compute the cross feed from vertical speed to pitch, with saturation float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp, -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max, @@ -821,12 +832,10 @@ static uint8_t updateFixedDesiredAttitude() ); // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -(airspeedError * fixedWingPathFollowerSettings.SpeedPI.Kp - + i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki - ) + verticalSpeedToPitchCommandComponent; + pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent; fixedWingPathFollowerStatus.Error.Speed = airspeedError; - fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed; + fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator; fixedWingPathFollowerStatus.Command.Speed = pitchCommand; stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand, @@ -869,14 +878,10 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } - i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki, - -fixedWingPathFollowerSettings.CoursePI.ILimit, - fixedWingPathFollowerSettings.CoursePI.ILimit); - courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp + - i.course); + courseCommand = pid_apply(&global.PIDcourse, courseError, dT); fixedWingPathFollowerStatus.Error.Course = courseError; - fixedWingPathFollowerStatus.ErrorInt.Course = i.course; + fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator; fixedWingPathFollowerStatus.Command.Course = courseCommand; stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral + @@ -1013,7 +1018,7 @@ static bool correctCourse(float *C, float *V, float *F, float s) */ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; uint8_t result = 1; VelocityDesiredData velocityDesired; @@ -1056,28 +1061,17 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) // Compute desired north command northError = velocityDesired.North - velocityState.North; - i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, - -vtolPathFollowerSettings.HorizontalVelPI.ILimit, - vtolPathFollowerSettings.HorizontalVelPI.ILimit); - northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0] - + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward); + northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; // Compute desired east command - eastError = velocityDesired.East - velocityState.East; - i.vel[1] = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, - -vtolPathFollowerSettings.HorizontalVelPI.ILimit, - vtolPathFollowerSettings.HorizontalVelPI.ILimit); - eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1] - + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward); + eastError = velocityDesired.East - velocityState.East; + eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; // Compute desired down command - downError = velocityDesired.Down - velocityState.Down; + downError = velocityDesired.Down - velocityState.Down; // Must flip this sign - downError = -downError; - i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, - -vtolPathFollowerSettings.VerticalVelPI.ILimit, - vtolPathFollowerSettings.VerticalVelPI.ILimit); - downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + downError = -downError; + downCommand = pid_apply(&global.PIDvel[2], downError, dT); stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); @@ -1092,22 +1086,22 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) if ( // emergency flyaway detection ( // integral already at its limit - vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[0]) < 1e-6f || - vtolPathFollowerSettings.HorizontalVelPI.ILimit - fabsf(i.vel[1]) < 1e-6f + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || + vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f ) && // angle between desired and actual velocity >90 degrees (by dot product) (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) && // quad is moving at significant speed (during flyaway it would keep speeding up) squaref(velocityState.North) + squaref(velocityState.East) > 1.0f ) { - i.vtolEmergencyFallback += dT; - if (i.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { + global.vtolEmergencyFallback += dT; + if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) { // after emergency timeout, trigger alarm - everything else is handled by callers // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...) result = 0; } } else { - i.vtolEmergencyFallback = 0.0f; + global.vtolEmergencyFallback = 0.0f; } // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the @@ -1146,7 +1140,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) */ static void updateVtolDesiredAttitudeEmergencyFallback() { - float dT = updatePeriod / 1000.0f; + const float dT = updatePeriod / 1000.0f; VelocityDesiredData velocityDesired; VelocityStateData velocityState; @@ -1182,10 +1176,7 @@ static void updateVtolDesiredAttitudeEmergencyFallback() downError = velocityDesired.Down - velocityState.Down; // Must flip this sign downError = -downError; - i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki, - -vtolPathFollowerSettings.VerticalVelPI.ILimit, - vtolPathFollowerSettings.VerticalVelPI.ILimit); - downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]); + downCommand = pid_apply(&global.PIDvel[2], downError, dT); stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 83bc91e25..8a7e5d064 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -15,9 +15,9 @@ - + - + @@ -38,7 +38,7 @@ in relation to vertical speed error (absolute but including crossfeed) --> - + @@ -46,7 +46,7 @@ + defaultvalue="90, 1.0, 0.5, 1.5, 1.0, 1, 0, 1" /> diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 7da8f9091..fec77cb55 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -2,22 +2,22 @@ Settings for the @ref VtolPathFollowerModule - + - - - - + + + + - + - + From dd07ccbec9b235da531eb8964cb0f09b70dbcdeb Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sat, 16 Aug 2014 14:20:31 +1000 Subject: [PATCH 135/718] Resolve merge conflicts --- .../pages/outputcalibrationpage.cpp | 21 +++++++++---------- .../setupwizard/vehicleconfigurationsource.h | 8 +------ 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index f2f79c5f5..d724f6076 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -137,10 +137,16 @@ void OutputCalibrationPage::setupVehicle() m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; break; -<<<<<<< HEAD + case SetupWizard::MULTI_ROTOR_HEXA_X: + m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; + m_vehicleElementIds << "hexa-x" << "hexa-x-frame" << "hexa-x-m1" << "hexa-x-m2" << "hexa-x-m3" << "hexa-x-m4" << "hexa-x-m5" << "hexa-x-m6"; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; + m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + break; + // Fixed Wing case SetupWizard::FIXED_WING_AILERON: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; //2 for servoCenterSlider! + m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; //2 for servoCenterSlider! m_vehicleElementIds << "aileron" << "aileron-frame" << "aileron-motor" << "aileron-ail-left" << "aileron-ail-right" << "aileron-rudder" << "aileron-elevator"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; @@ -148,7 +154,7 @@ void OutputCalibrationPage::setupVehicle() // see Servo city for an example. 1500 usec is center on MS85mg for example. // - http://www.servocity.com/html/hs-85mg__mighty_micro.html // make sure Aileron servo one does not go to an extreme value - // would also be nice to make these all default to 50hz so we don't shred servos. + // would also be nice to make these all default to 50hz so we don't shred servos. m_actuatorSettings[1].channelNeutral = 1500; // make sure Aileron servo two does not go to an extreme value @@ -184,7 +190,7 @@ void OutputCalibrationPage::setupVehicle() break; case SetupWizard::FIXED_WING_ELEVON: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 2; //2 for servoCenterSlider! + m_wizardIndexes << 0 << 1 << 2 << 2; //2 for servoCenterSlider! m_vehicleElementIds << "elevon" << "elevon-frame" << "elevon-motor" << "elevon-left" << "elevon-right"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3; m_channelIndex << 0 << 0 << 1 << 2; @@ -218,13 +224,6 @@ void OutputCalibrationPage::setupVehicle() m_actuatorSettings[7].channelMax = 2400; getWizard()->setActuatorSettings(m_actuatorSettings); -======= - case SetupWizard::MULTI_ROTOR_HEXA_X: - m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; - m_vehicleElementIds << "hexa-x" << "hexa-x-frame" << "hexa-x-m1" << "hexa-x-m2" << "hexa-x-m3" << "hexa-x-m4" << "hexa-x-m5" << "hexa-x-m6"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; ->>>>>>> next break; default: break; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 099126aaf..733518ee2 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -58,15 +58,9 @@ public: enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK }; 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, -<<<<<<< HEAD - MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, 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, FIXED_WING_ELEVON, HELI_CCPM }; -======= MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, 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 }; ->>>>>>> next + FIXED_WING_AILERON, FIXED_WING_VTAIL, FIXED_WING_ELEVON, 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 }; From 449653f01e4221f104579d47b25e2b0d9664a890 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sat, 16 Aug 2014 23:53:02 +1000 Subject: [PATCH 136/718] Revert "Merge remote-tracking branch 'origin/parched/OP-1324_fix_build_when_python_isnt_python2' into next" This reverts commit 4592de13bda5c825fc2a6065e8e4978330ceaca2, reversing changes made to a309537e62aa938e483f8b21204f95da91244ed9. --- ground/openpilotgcs/src/python.pri | 2 +- make/tools.mk | 6 +----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/python.pri b/ground/openpilotgcs/src/python.pri index 62d03795f..739fc2cc9 100644 --- a/ground/openpilotgcs/src/python.pri +++ b/ground/openpilotgcs/src/python.pri @@ -15,7 +15,7 @@ OPENPILOT_TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) PYTHON = \"$$ROOT_DIR/tools/$$PYTHON_DIR/python\" } else { # not found, hope it's in the path... - PYTHON = $$(PYTHON) + PYTHON = \"python\" } } diff --git a/make/tools.mk b/make/tools.mk index 17a22ca77..913b90901 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -690,11 +690,7 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists) else # not installed, hope it's in the path... # $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH) - ifeq ($(findstring Python 2,$(shell python --version)), Python 2) - export PYTHON := python - else - export PYTHON := python2 - endif + export PYTHON := python endif .PHONY: python_version From 22e981ea8784f7c952e2c9edb742b735e0f671a4 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 08:34:45 +0200 Subject: [PATCH 137/718] OP-922 Mark PIOS_DEBUG_Panic with attribute noreturn --- flight/pios/inc/pios_debug.h | 2 +- flight/pios/osx/inc/pios_debug.h | 2 +- flight/pios/osx/osx/pios_debug.c | 5 +++++ flight/pios/posix/pios_debug.c | 5 +++++ 4 files changed, 12 insertions(+), 2 deletions(-) diff --git a/flight/pios/inc/pios_debug.h b/flight/pios/inc/pios_debug.h index 8fd7016a9..4d481dfbd 100644 --- a/flight/pios/inc/pios_debug.h +++ b/flight/pios/inc/pios_debug.h @@ -58,7 +58,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin); void PIOS_DEBUG_PinLow(uint8_t pin); void PIOS_DEBUG_PinValue8Bit(uint8_t value); void PIOS_DEBUG_PinValue4BitL(uint8_t value); -void PIOS_DEBUG_Panic(const char *msg); +void PIOS_DEBUG_Panic(const char *msg) __attribute__((noreturn)); #ifdef DEBUG #define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } diff --git a/flight/pios/osx/inc/pios_debug.h b/flight/pios/osx/inc/pios_debug.h index 1d4b88595..460bf7d0a 100644 --- a/flight/pios/osx/inc/pios_debug.h +++ b/flight/pios/osx/inc/pios_debug.h @@ -38,7 +38,7 @@ void PIOS_DEBUG_PinHigh(uint8_t pin); void PIOS_DEBUG_PinLow(uint8_t pin); void PIOS_DEBUG_PinValue8Bit(uint8_t value); void PIOS_DEBUG_PinValue4BitL(uint8_t value); -void PIOS_DEBUG_Panic(const char *msg); +void PIOS_DEBUG_Panic(const char *msg) __attribute__((noreturn)); #ifdef DEBUG #define PIOS_DEBUG_Assert(test) if (!(test)) { PIOS_DEBUG_Panic(PIOS_DEBUG_AssertMsg); } diff --git a/flight/pios/osx/osx/pios_debug.c b/flight/pios/osx/osx/pios_debug.c index 71120a39d..bb6cd0d9e 100644 --- a/flight/pios/osx/osx/pios_debug.c +++ b/flight/pios/osx/osx/pios_debug.c @@ -82,6 +82,11 @@ void PIOS_DEBUG_Panic(const char *msg) int b = 0; int a = (2 / b); b = a; + + // Stay put + while (1) { + ; + } } /** diff --git a/flight/pios/posix/pios_debug.c b/flight/pios/posix/pios_debug.c index ac67e89a0..1f79c4722 100644 --- a/flight/pios/posix/pios_debug.c +++ b/flight/pios/posix/pios_debug.c @@ -81,6 +81,11 @@ void PIOS_DEBUG_Panic(const char *msg) int b = 0; int a = (2 / b); b = a; + + // Stay put + while (1) { + ; + } } /** From 34983a2d2887b05088b90bf72fe41611f3644ec3 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 08:40:37 +0200 Subject: [PATCH 138/718] OP-922 Fix typo in pios_tim.c --- flight/pios/stm32f4xx/pios_tim.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/pios/stm32f4xx/pios_tim.c b/flight/pios/stm32f4xx/pios_tim.c index ec72a9c6f..a5b114b0a 100644 --- a/flight/pios/stm32f4xx/pios_tim.c +++ b/flight/pios/stm32f4xx/pios_tim.c @@ -171,7 +171,7 @@ int32_t PIOS_TIM_InitChannels(uint32_t *tim_id, const struct pios_tim_channel *c */ // commented out for now as f4 starts all clocks GPIO_Init(chan->pin.gpio, &chan->pin.init); - PIOS_DEBUG_Assert(chan->remaP); + PIOS_DEBUG_Assert(chan->remap); // Second parameter should technically be PinSource but they are numerically the same GPIO_PinAFConfig(chan->pin.gpio, chan->pin.pin_source, chan->remap); From 232d9c10b2f4308864ae0b0b6f774a27f99cd0c9 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 08:47:04 +0200 Subject: [PATCH 139/718] OP-922 Make it explict that matrix_mult_3x3f should be static inline --- flight/libraries/inc/CoordinateConversions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/libraries/inc/CoordinateConversions.h b/flight/libraries/inc/CoordinateConversions.h index f64823a43..c8a391f43 100644 --- a/flight/libraries/inc/CoordinateConversions.h +++ b/flight/libraries/inc/CoordinateConversions.h @@ -97,7 +97,7 @@ void rot_mult(float R[3][3], const float vec[3], float vec_out[3]); * @param b * @param result */ -inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3]) +static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3]) { result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2]; result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2]; From 4ba9208727b6bea00e7cf0a7b6e6d8fdd13ec2af Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 08:47:55 +0200 Subject: [PATCH 140/718] OP-922 Comment out the unused DEBUG variable in rscode --- flight/libraries/rscode/ecc.h | 2 +- flight/libraries/rscode/rs.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/libraries/rscode/ecc.h b/flight/libraries/rscode/ecc.h index bf56a0b9a..0a6316d8a 100644 --- a/flight/libraries/rscode/ecc.h +++ b/flight/libraries/rscode/ecc.h @@ -66,7 +66,7 @@ extern int pBytes[MAXDEG]; extern int synBytes[MAXDEG]; /* print debugging info */ -extern int DEBUG; +//extern int DEBUG; /* Reed Solomon encode/decode routines */ void initialize_ecc (void); diff --git a/flight/libraries/rscode/rs.c b/flight/libraries/rscode/rs.c index 39c014720..62fd02386 100644 --- a/flight/libraries/rscode/rs.c +++ b/flight/libraries/rscode/rs.c @@ -38,7 +38,7 @@ int synBytes[MAXDEG]; /* generator polynomial */ int genPoly[MAXDEG*2]; -int DEBUG = FALSE; +//int DEBUG = FALSE; static void compute_genpoly (int nbytes, int genpoly[]); From 3d8d803c337b9277017e54ebfaa373fb581b5d82 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 08:59:10 +0200 Subject: [PATCH 141/718] OP-922 Fix missing ppm_id to ppm_out_id rename --- flight/pios/stm32f10x/pios_ppm_out.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/pios/stm32f10x/pios_ppm_out.c b/flight/pios/stm32f10x/pios_ppm_out.c index efb6a6a6a..d47934177 100644 --- a/flight/pios/stm32f10x/pios_ppm_out.c +++ b/flight/pios/stm32f10x/pios_ppm_out.c @@ -107,7 +107,7 @@ static const struct pios_tim_callbacks tim_out_callbacks = { int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg) { - PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(ppm_out_id); PIOS_DEBUG_Assert(cfg); // Allocate the device structure From 0b147fded5033b901f0d71fd1844130196790bee Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 01:41:14 +1000 Subject: [PATCH 142/718] Minor formatting, add wizard image --- .../plugins/setupwizard/pages/autoupdatepage.ui | 2 +- .../plugins/setupwizard/pages/opstartpage.ui | 2 +- .../plugins/setupwizard/resources/wizard.png | Bin 0 -> 22382 bytes .../src/plugins/setupwizard/wizardResources.qrc | 1 + 4 files changed, 3 insertions(+), 2 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/resources/wizard.png diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.ui index 34c29e8c0..782d24842 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.ui @@ -21,7 +21,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">Firmware Update Wizard</span></p> +<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;">Firmware Update</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;">It is necessary that your firmware and ground control software are the same version.</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;"><br /></p> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui index a87c2e18f..589be9eb9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui @@ -39,7 +39,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">Welcome to the OpenPilot Setup Wizard</span></p> +<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;">Welcome to the OpenPilot Setup Wizard</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;"><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;">This wizard will guide you through the basic steps required to setup your OpenPilot controller for the first time. You will be asked questions about your platform (multirotor/heli/fixed-wing) which this wizard will use to configure your controller for its first flight. </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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/resources/wizard.png b/ground/openpilotgcs/src/plugins/setupwizard/resources/wizard.png new file mode 100644 index 0000000000000000000000000000000000000000..59bcdba13df4a4fd306a438220723869635388fe GIT binary patch literal 22382 zcmV)wK$O3UP)Zo!`L(jOfj8=UP8zv zof|?DZhG=1xi|f$64FR=AtaCx5(ohjLcsK5V`JQHS+Xoyy`6J(>XiNcF{+ki%h=*x zAn)_cbEKI)Gkf+sYwxw!T6>L8(qB$j}Tz7Fv>_Br^{P50&fb8E)pc~lO3AL>dzd}m6W3ua|O!f`l zOc26zdcarm^51^F^pbU_EiEW5bc=IN?LD*<5CzUGfa~T%>@gw4(n+t|za+|r5TSQN z;kU1U*P5d4UKbDjvly?(;Lo=|#ici$L^7peOO(ZsbV(o$+}i>@mrJQXJ8AX%7f~RD zb1{@Y3hyh4gbaG(8Uuus6s9SRWZ1eV%xIDZwhZuxc8FglW#;KgspH$7C?P}yqOZV* zrh=ETrACh0FNDNyyMcXK;5P>#zDP=WXn%Rkcw3KALI?#y56_)wefMUQgu5U%Pen@^`MY6w4AvD7^)E7I*>xMZgX3+IC2p z3KeQr3gNl_c-Qr9OOz1atG@WrdDmZkk#zjj?#PPF&o}xog^3xwsnuMUnjE z%NywIaSuBy1atu_?J`253dwaBjg%_C*W{d&YD#?`&v%Yzoweh6u{RIGKWk_@ry!ZC z_5M^~@U`u6fg2 zeL|#tr=QTu`D=D?>au;@@~v~#ROY}sAw-pwvf+5t_-#j&5Y7`|_Nr{ILj3N*3=jUJ zmi^cSPUY#1 z)quAY{_+#x+~ZO2w-r%B2mz5h;GKopyuvU4P(I>#Dy1@@eqVJ#J%(ZNonOu)nNm4z z#X&ah(0qqpQ%e1DV2Kdgx@TSq{GvBmQU(Yp6q;JJR0y$5O8LsM*8OcZMtT1YR#rg( z(-iF3>mNLF({=m!!524UnINU)nTj4g&ZW8C!^ny5AKGBgLMdhY5$<&o1U?Jy#b87rag~(DgU3?;w^7#@`W9SR3MzKP z4`Iu$5LkmkS{tl2TfSJMVCK zTXcOwh^iBplwam{&2=29tdwx){Wa*uSWOf5V-qzYNGTIirhg!%am}%i1aA|fAoTUG zd}4mVrRN@25C-v^2d0g89S8VkAHRA{9B+A|gb>9LTr+#R$&FVwjZ+5HVU~&RxO3Ri z;bV9ZaLk&NiXH)Pd7>cnWjMFk<2G5fw3qoa`vBYeuzFcH-~7}zKKGHGSQFBz0)Uu0 znYA4cZ)s)1wzrjC0CN<#s|BRwi#PA3rOm-tYBi=uNR?j0pd=kLPhw6HA61!uHjSte4NtbZ}a4gawM!xW6{;=rL<=*C!6CT%Qr zaMJOX?fQfe-o^7`*Z=4%Y2`haHV;aS?4LWmvNgn&A3cFwe(WZ=p#>=zu#N4~g_zSQ^~%Ic*pEST9jVxOj3fL|3t#3r%gW8y7Iln}xR zjt~9ypDVTLC!ff-e^!MpOEzIr!p%3-4m)G?_>{_MeDdF31z_HcF23=JrF9>Q>9Eak2b%r-;`g(;^ro}9_u=V) zq^ZrvzhAqFC3D*P&kO=`JR8%b;Vng!?aFYS0z<}wn(8!n-CxFI&lHX-Zcr7*m#Rgczyo{?--#vyImHa{>IUp z=K{Vww|;9irX{%lv6(#Zcn!fohIGaSs1peSJg#_)5#{~}taO2G%Zr9|UfmI3DAIM} zc;Fw?IM8gLJ7A^giM#u+qq>fF&5wk)3{gUe)n}Y^dhvqU?W{Smn@}Lj6(5;Dti;g~ z^~c9*2LGO0(}|Q8uk9Y_KE0|CB0L$P%j4lKG)Bd4{pu$xg5Ud6EhR-c-u1zSw0Aj= zeA<|h=)=aXeFwsP_f!AC<1+g9gf-saxzt;NC?SM%{;bfdRm-x}H@Z0MnniT?j_=I% z`wW)N?`EP>)|YnTbm)Bgeb4moiTd~#QLhK;xz}I1Bmz)TnxnKRf7sFmksR+j=Ku?5 z^-xvTH=#T1u8(oed0RMjc|8#Hg5wSKOT8tC($wh3*S*8eiw&?yZ%(oy$@B=(&6Eitx-m=;uyO-++807Bn6Q^ z?)&e*0qXS0c%Jd_rY8!#R~=}c7kKyyyCIV)l^Z@ekM7>_71~^0VaWyO(U*2Gb~xoo z1bjK>Oz-H|@+Hww8kf_0L&vB$eE_Bqg|EQpmV$?gtP})^+hvf=i6fS+ERA!+C7ZeQ ztk>|kv$kj0GO;X!bsr4vuRqxPhK*Bi>Uoq9q6FrY4lU`N7_zz1L72l)Q`N(*?|qRo zPpSoJ^pj&d9%cW1iVMUWYMppf69u7vgEd8}DlyH`N9WE+j)*UxC~6xbeC%s$sVH4e zWoa+vB|TIW_i)J>TkyK=xm0mfbqFB>Qp!VDvP}x!)MM1N%Fqoz{IW+?m9eGi16S|w zcb`ublGRI^0m$Su_Uw=G_zTncpFc0*t3Q1Q2U^2`WN$+u+iGJLZ{S?&O-qyzV#cxs z(+mFJ_onw-zekd#D3VLgYN4z+JI2{|*^#u{f{rCuBmt+ z4*d_b^SbSm%LhaOpkCBcoUCNj`Iw|_J|imb*{XynGIV4Y}y(?H{jegZScv9J3ST6ViqV&j0zT{&Ahtw3{mOK@;$9#i!1snt zdf$}lxe($6Soo?Q@J;3iDlCtN*H{zM! zq!Q)+3A_-6#|QW9A3Og45Onu%L(p}2-{)c7YFM)r45fk{4RLP!eFv%{(9Ik*70q0A zb~7h07To#QMmB77lFF0<@(mlK-gHC>Av91H9c~`<`mPI#3Wi!vb%nPr`=UaCD!>v21Va(Lo`EBhw$3z} zJT$g}ra~pI~;6pss}1?Qgvey_cqvBEA} z7?waQ5by{gyi&^XRmi1$G@4=tTAry5RQ4h4f9o*aln-!obD8o>W) zqJ$7WU=FYpoQoiMHaOBCR1nhc0;K?he&s^gkEAM)S0N=PYOEE)&KQ=@=fTS28Me5l zu@#!zq3lxFSr6rd;hlyC>Y*uKCI$beh_Vy!Di=cZ{h-c)nGRL05T;oypitQJpD^IY zPGp!lWJ?2m(*8{X?G_{i1W%T!n-po-I1x6sk;V(YFTL%t%5k$$65 z`^2z)V(3^xKs%rL;HxD2dgX5(h!gLN(@!4V@jOyOylXe4a{SjvhU%Ej!|wg?@^+YB z0W&Kh8icO61jNEOqMiO%h!R4$zfkVXW^VKe){#7 znYHEuMg~w0IQ+y>?h61uEUSYr+|(>CKC_T3ZoYxFt9P+>^$wodp!4zXcM%PxST?7F zb5BxOwa{XAHMpIyXr{e^?b45hpeqhwy&=xSH=HOTe7}Oy>tLM*7t$YePc~=qVq4Lt!L=v`qFj zEG3b;nncQEe{%;9JlR0dw;PYUjgnZNcP!UAYgLYw^C9Gif(Yz60ARf#s}J8WTb~f( zG6>%dCp+O245hjm>c*y&^s)c>-=k@^)#0j}PT}6aRE{b)yt|8pvRrs(6L9JAK*()_zW=Mnh+y?i(;AnWQI0Ac zc@KM4h8$e~B&FoM({}eSzZvUXCoX3hn|EG7E`KT6+=tPPYk;$WnpF_X3lT~Q5!xq& z^KK!;^|oSkwCyBHg%~@J#iUD=5dPnR=PI~^swr*`Gza=usE>SWKYGJ!eB(14@VJc; z=j(>T_wSg+SAH;`zLdhD5^~zgRvx+Y38tKI5t?Rj^72-8*9F*F8{$w&I(0=Ow}1KX z6oivlmVuP!h=7&dbrC-P&2??%h8 z0}gEIVOt6Sk6WR<)Jb_s5YzPY^vhA80Q>tH5xgFiPk-+)Z*b_o0q0mFDEQ) z;@S&0BQ1mXfAIqPQW^nYhM+G+z@O$obBMn`H;coOB!r|elAxxtgTAzr*Y*@0Qhqef zII)iRy=xO!-g@a_TF(Oho`$+LQpyc5sal`=7H~{~8v&?q4D!BDolYo_LsK^p_1?!P zPj*w{vpogP$ysb^T@N&46Bg^QUbuf74v9VC&63IXu(!UAz4a}?R_wP|!=4m7$!0aa z`~xS7aH5C^QWlUmi`aJ{!X;}pR8{hi=W6=1w~R%|q1xln-%?6WSkS~5KlBf_*A{X6 zy(`(Ut&EXc>1ZsP)y9d7ns{+bIiWz7l2{9|a0`3uQ*^{LKq3#umj_6KNtGyXD@2On z_YCMPn%2vrSzjm9R>uiboGhOez-Q)>{TZXa{G_CzcR!$GYlSiF`A@EY%l7|{wFtmL zz*S65G{~Y7g?U8=G4GJi%97BVG3d@4bfzUuacE6Sazdvou8=nrOzFq8qGXL2h84gL z>hxfLCBVbT%$fFYO=UaZ|I~y0?b%t}cF#&i520h%v5}XzsMIy2a61f& zJq{L@Y0Rw%vbohV|GBOGhUR$BZ$`A|WDsRfXtgAq=76sN77q1Kgk1NscB+ckVQY4( zA~%f$8(dpur=KdoNA7X<^-5mq%bAmBxgb_>cJOwk?zg%@dx%L z4#rcVQEfU&MEQXPlnVGHVfRlstvbi~v)t@#H|fm`-P7$NB$i|WuSeAulSeT}TX;@gMg%BVV1w|Doih}A;(HvvXRt()j&*sTx z400KRL{9L?ZjB8Glk{dJ~TswRUGJZ;lF@nJl7C2A%<7ykqJV?czBu0xr7&YdcL^lOx^e0guY+MPYny z1wCup4QmdB5QCzC5DG#8MO9EV1y!{hRhkmRFfnxtLpQNZn-I&8Sf-s(Y@$#!1(!?1 z;dbD3+fq3SQrd*2ZQ}B}*UTQLEj3>#NIZP{yB{K+|6!=P5QtIP^Sq46*{oINo z@CIFUW?#;Bkmu*DdxAUr0;PEoeO?2soKB zdHm&W=1=o7t=x+uMpjEounY@BH_&qiM$*DGOpLsRWgWlR3Cpm^7j_s1*hAA z)1%>VYxc__1TL=wm)GvT^{m0Vn8DiF5NSwTT?QJ9tgU|9wSUkvZXdfV{@8mMGgXP6)7c_DUqgi zluM;;*sh4zapFLo42w)(j!d5&8>nguPLC~ZPPc}lDmdIWiG*wmhoM^-IRn#_NXtS> ziRw^sc^qgCyH`2bmSw}P1eUZ%x&ab4xAZjt8^)^3WD(^6Ff2Lh`fi6H;1VQrW4B;; zTGF#WPsFQG9hjLH%ZPOJ5#%VXSi}izpqyu;iG_(ksIDAUYl-(4DsEPFwg~f+C+np~71GJLIto{X^s6{@z@SH&~*!0c3Tn zbgY9>@8P9tI8r%Dea>WmH-tS3MSeldr=a}b;tar6UQ7#pSQK^6kTmvMv?a&xz6*W! zgCuiiSI)4yrIcgFU{4lNuK`Gi_GXRtuv;6IFn3x^WowHu!R*Mkl=WmRdNQDNnG|>x ziv5C+=l@_Jm4u#*q$O$6osr`jDFQBm+hG&6vm?D9cy7E)PS#V_VFxX+r!zBVuyMd8 zm{q79ZvHtgEDPHEEVj0qyn4`}Gi4oZVwOpPZbDPsVq=rR_BMm=^!OwJlm>^IKWADd zhxwGmq)3#MQVJn<1Iu@Ir29@N4~$tZ<`Yb}RQ7eg{(#E9yku9qiCZ(Nj;a*+-dG9C z8IrEFq&sa9&&mlaFas`hs|?S8n7!{-)(X%Q=+T^ zyV^|(e3I!g6~}Q8dhUmWWM0yhvgpo8()ptoWuH?}Q#i7MF^}fqe~~ESIpc_nl1l>$ zQIA4hmq}Op*e0-@DT{d4qNYG2<~@EIM>-F!Nt4c$MgABfsv};7Sw$+^Ayu1S?3*L( zIO#DlnM5@JSaM?Hpi6@}#VTECL4B7+)_DDcj&8!vHiPPjLV5W0iY5f))e6kxwUp7#8N5crcuX_>F$St+o?=@-6>*T*jc zMiDh>BX2fuEsFRYyyw)ilIb-8X3h%Wc2BIq@`fatmGtE#`F@gwu#K5Vi-U2CeO*V_ zS9PT&n-A*8M3SmNv0q_nxyFowBa;MhN>!)~JBDr)Cd+#61sbD%NAw?`T54*lPysAU zvU_KObb9QD%bU>HYg^Y{nqcY_7t0m|nKL6mDCR>rTx2o^yZ3anbw@8d_oj&Vjc?%? zju{0S#epLe-IkQ>?VQ*(4+pP95c3HNdbeYkE)D--8m%W|vG*vZ3W_2qi8-jL z_OfVRi1K0=U0oSk+w;7(GeK=*j>cYts8^vnqT(Gx|5b#bywXEKp__eslavOWdk-e^ z7Xm*TUhcR{l)ZG5dZ2t+nScF9R+g<%RfY0O53!gFA)vmlkM1tp^e=(>E|Zpo#p;EA zF28g-j>;J*v9dvOjtD8SIvc5dek*tXv4Qp;eavEEw_twh*zaaWmuzk}jyOf|xD-|_ z467#gPR|BhYINY3q6$JG2Tq4VTU!<+ z>})rQ=PWKiyNHw4%|or4Z+i+~4=fY2{xu%?<2D|7wr4QFayXVu(Z=kGwzV1~JkJAg zX~K@_5Q2AKRLqhUr6|?&5rOEavSU`Mvk9wl7ytNY3x9p8hp-oB6>2B~`~i(wvx6Wd zdv+&DCJhofv+HAz>Jt@V@cF?YH^91blRZ3J%?pVdb)>P(e*th zy;+NoTs?)^YnGvu*g>M{zAUdiy&IX$AoUCg72$N_i-uWx$_&Q&6fP(#>hvX?cdef( zC7bx={jE4PK}H`U;9%Th<{^{KuCz6RBmqRd3I%>aLyv{mt8()Vl@!gX;Xo$BfhX%g z-vNdWniHHJ<}9C1Vdp03vTVe6qxnZe>S z7IW^&Q8I?4I3NbC?HwtLyg5>Gn&U(LoyQ?qG(}@}kxFaALQ~;WAE_aB!fMuU%8=f> z1HU&+)tXyqFz52rGh2|Id)WWNMqd2qwz0qL=5(XZT+X~RSMuSj%Sq-e>bp$haf9t! zd+F)U<94eAgAM`#2T_mK$stMbkGFb$A@+FRx3JhZ51^qTO(L$Nse)Iues@b*NM*?sTj6Moo22vb;cLqY z&Ys7UkL_h{u$>>=^BS=eZ=z=5YTkXzS9s{<95;S*JNG`koheom&p)gx6nUbN|et*JJ4Wckpy zx3i!k$1@LYJVI3jYV`uHxvUZ?1zAH<9kIJSW0-HHJ8fIut0F4XqPCwaZ%PjKN={i8 zq-t&@yP7O&A}Rj+o?4!GrJGkb+V`%nt);ieUV;6W=Q_CS(R%#34i4_`99^A$C>3*9 zeR3%?%3bX5ws5&s6csu;a_rm|ClS}F2)Q&SnW(7%Crk;9a(<;#1|97=T3T~7_nMrx zJcMWJ3_A-4@XGq_oLHmtgQJ%V8 zEr+j-<{(s4!-)(0H1?X51_gya1!I^7-j%i}@+(w?6|{Z>Tw{-g$0fM%+$pHl^J&}L zK%%#o2cEIL5VwBl!(4FcshoS#$s@YDyB=*J>P)eBQ!V2{)ljP!aMi`7o{pX9Pqr zM$xRP%&2hD(KmWNpxeQXlQTy3&!5Tx3cQZdpO)!IXUd|y(1|lvY$tcX4v>_&3=OS^ zf4~8>_vFdtP2>?81OP=PP+Ck;#6bpu$E8rH(QDw4I{Dn#bGgB4g$h|ZKS&}gF%IF%w22zAlJR&-R5^CQ zc+TTg0-=zD;u1F%m0n^6E;LQ_$A3!}%=RH7LrYAA5J;&owahc_#fPIL>cZ<%28lir zM7WUoHC`Iqb;|nh5ezv{H8H4(miF&Y=#sEsp`gfzaCr&E{KQihbE_wo_Gs=@AG)RB zbRFTd^xhCFmW5G-pf@v;seXw{4c~V>i5f7FIZRXP0A*z!ii+J-PxUc>K^Skqfs{~P z;YIj{<{Ux@gxf=9k(((6NB)q^iSt9~mO%4Pj9B3dQ&sLIr(1*~F5GUFaM*6hXv~S< z?;sem|2Ks~B%mR@wpZHYb&|21tePJt8a#aCR~EThFx8K69l2h@?Zx4A;BncOdcWU6 zd8Li-aeD%jw{&H`btxQ#nl){MY+y=+hne3nyl^5Z(|TmqN~fzLZWf-rrpc1dteB z_IN83djM*?GA#hvjLCt96nl0h*}FSQOM4DQKq%(n(5RM`XU^lD`Y0vp7tD4|~bj?9Qp-o0QYY_}N1~ohcxIIJn%IOx7h!dPcsasB7 zx0q-4I5>T2l;3@60W&K{c7uy+0^I-AMJ$^gX7d3TE7nXbM~HBFa5)rvx*#DQ*Xixi zN#(48b*4j)FtI0{Fm?k=pV}YKo?JDCIR!f&+sK=vZtQew)U24xOIx;c;Yo#@I5)%- zo4VLum%{5&m^U@R*(+j5Mdk6=6xO|a*~DulC3@bLXH5~jyf49B&o;CDV2V`MM7Jah z%l(`*D~uBd>8u5rp-)C=4wZ$cE#k>%x3H=v$b(;7Oixm0e`}WMWgdb)jbu*YiCr#M zoj>t<$xM=L-okxIU};k#r*Vix9dDwZ2Ck~_$<>T?(5czkZ93Z1R7h)-GBABkDPEtG zM<3t9^64s9o>2;C*vBjhyIWKarv03A?Q&f1BMjEVq37uC&S6@T&)mP4?!G*iEiB^d z*@dV=Af-iZBFCo2K3=R#aCvSD*QQ6b6H}^V6qcRJa}U3Ynu`9RI z^T8w?omtBBS%hnJ+)HU?jPtI42U|DPv8%Zkk;JtD4D_C@E!^5aMsZ1%KV$)pXOU zDK#@@2JrjseBrsgg;N!5-`&UJcqgK0?8ID(Dp+*l@%xyJOm^|5d-hNqaN`nAbklZ~ zkA-;!9{%H$a=!Z8J)C>RK`Lv;p1Vg114IhPj#r#eolyl_!$C=NdS#IR-guB_)^FjIHH%)?y}NtUJonN* zo_=v3yY{vrrDVpmA}&3plwd~_O&vOOibqngt22`Y>ZFt-EW48x+;TPWTOsKD$ptff zPF0MU0lv~?5R0l9rG9?>XdQ`UhPgGi)v2Q=O-EM>T{l^=q>44GXK?DO8ctkRjpt|- zh<@~K-$3=nzvuGVl{hs?S3XBbbF;ic8x;h3D4sl!z*pwuhd=(vguTv&VeyB*Y~n|E zJjJH1P53+x=FKc--t;iJoXM8m9csIEaCIFoXzy=f(cbZ_Vw^T_iko(B!DF)hGNp(m&F%w(a>cwtw25O8ii>= z7q_i%qbcy=ch2EAx6Gv~tRTd2VW{N4|4`4D?%9J= zSj;Oa8mvPyXLk2yjm5zC554|mW^P#vJO+rX7Z$y+c6xYKMz{D=Z8smgd>UsgomgWJ zpsw5EfsF}%{@`v-KV>GLyZLOYDh^`-$Y;rHd7cYD`8?%54=zQcE)^$Znk*@Zkk1Q> z{K_b|{Gqt{kve|y(S=-m#mOkeQ^yRrdFG{kT>Ig>m{uO*mUm9)oOw>hj)#STWFoiz zWG6rVdjmJGn!=(IdjfsW){dwDQQNZuD3Vf+_)6xa1z<`k9|yh-@VnRA7aoY`_HFJ= z^X(7M9VE%JAd@zT_vVTBKAJd`=A3rP1#_z2p>M&Sz`Zu`g zGxyP#8e`p;X<*iF;+J>tBb_sC11?~ikWMT7y{-+1W`_f8?q9Yu-s>9iIQZH9O>BR1 z6Xvd$klCU3lTz}{+aBkfD{kk;3up1z=jUx{d zI};q--^Zsfn@VNW&9e< zNfM4ULCb}qD|F~-_O<5-`4xn)SX2-um$$FybwYJSBYMcfg70r=rZnK8(&OjM)ltq_ zTZFTGCUJKW@4o47o_l#8zx>Qsx0&dknl zJkvM_xKK*@(5SYY{4{W43a}Fh+;B=+?X4G8*Jzp`mp!^15MUcJf?*fYm>aEsrQSCm z?cugRY~$$%KSoW&BDMQ9?zp?2eN9=`ofzW&S6lJqoEW--sT$P9Qygg3aXFx{Km!xz z76egD72TACJ%Xtb6<@zr_`_G4nOpAXtQkeTu)PnTN9DAYA-?)xEuBf7KYn2ab4uOx zbY+ zJ#%^T`5tn4i_1=r(4EPnDT0BH&8KSY?@poMB&S1Di$P_nh5|Ha`bZlV4yT_jt&+;H zLU~98=~=Aq4<8M(A=0BMUv=G|06U7#UU^ZNj7be%b2552zmVu^Uhb>BEZv9%D<2I z%*R!t>||Bk3tX%UVXT=N-m{=M;5JOHJe`;2B|h(#f7G>2{oM<#Gh2Ie`@VJcjA;&4 z5&4|`psK1MRDm=lrg>y3lR$T84%3u8`n3}{{ZlUxErhd z=ajg;JiEUcpTmJuKzq`5u?yhw+74k~Bh7;J9JbLk22y+;pFS?u9E z|2Bt%4apJ8j;abAE(N3@pEEK0$Ed?~?v}K8aesHSZps?qpND(a;~6ud0Cxiy&nocl z|IRtnjisg0nTd>Ana)cEpd#p9bpDL876Bbk?ChET$-8&AB$F9yfE3fTF!FXTB+aR! zIVV=Vcu$^uBF_&#FqiNC?EpV~qJx`1`56quz_Kh#D=G%9?UsbA7EeXjG`s~x=t4nL z1*#%Q2@h9%=<_`M*c06T(_b+6jEle*BJL>Vn-4b<4m`_2?X{9?TF$FuAE7I5jx^5EW2o>X3a zFfQF)snJqV=bZA4B1Gf*-SM)^ezLQtxhlpC?XxoS77mAk$Llzv z`ydg|Gb854>sEPbUxJd7lEF}}%P+s2O+DtIuQHd(ammye6=ApiayZ>6n!@bavpMOc zldvp{TrS5Ymt4Yax826qzx{ol-EVNloG7`BiQA*0s*ul_q|*lZtjW_PSXx-90wtn zqw?~(6)ulMeWj&0A3OiHo!)ylbT#z$BxQG3FS%@f@UeB>B9qn;NIYH@$6=(bFQM~= zcTJ;YN+pJ2VD^8Uclq+=n1Kk=Gvt}oO^Y%!%?-gooQVHFk@ z5)1~p@dF>>zL&enW(=|!W6baVP6XLZmhPTD^NL^8TGC?w{DNpzUtZ!+h4}dDiii+= zRS0p(q30blQFiWFegr5tE-&@><)!ExLy``K*P#|&HLr5n{G!m)KMMT>&3x0x^vi{73%iA2VF?{Dhk8QnT~ z>D+Q32;bILgH%ou^{U=$7L_yrzY#)=$jWv+B|beVDgXJkuk(1PN!H~ZQ^PEGTkPMzf5bj1CBMDJPuBj?CT$&OYQC64K zc9|#wKEAplEMTn=;vECWk4ZfjLO6koYoeZpl7Loq;3%bO#I-Rz#TT=2<3?IqTIlTTB%My<@pxD=Wy+z?h|A?-qPc(!)39^=WwKb7MP6!|_uf?} z^M>WU>pfH3!(KIT#n0=_tLBgPZnyVY6#5jRUe$MMb!g91`{Q2+PK0BhN38|ID=UJX z0P$m7>JPA{s%T9z-~4n(DtGFQCl&7f`;K_vo|ikP-1BlLRWVoZ=hjUzrbg9RCX*GJ zOcq5^aC@A%-7XwX2aR1hqR}Yn^yptMnp z;?+2JW@yUq>pu4S?2^XG@%r9lzMr1Oq+DhmP}iv2;!0SqT+yuT0KCxjRw ziCee4%EHniqAbfIpEt@>-pBk(Px(_xcvhM_rnW$+1g+50lXUGYs zbUPA+K)qzvlzG41b|C&*OTJ-Ysk7>u<>A0J%fqs+TW|j9Q|UAn|nh*kiz>Cl49qc_nbVm7-yPs=Oc6KIS zoKp}yarnI@(2|r)iwe%FiR6B{spVWa_O5SQz^V3n9U^?J8BPv`Lsk5zRTb{}^q-p+ zJousM*+{_aHVsoujmWZZUR;JDMeenO*_QQt62^12eI<|Y=q-7CM=x%-o7IhtELgB$ zP&KzK%TBv}h&w$H2#`n&MP&^fKa{B2eRUKsFV45cjZA%aF0-*AWj(Pg9_mRMK0p(K z$f~LS`tMy*{@=&_%bwT{?KU?+;2 z44gB)WbtpeHsrRqWSbUiiHc}Jm}o47t{Y_1Su&X{(vql1S4BuIdH?Dt`RDt1ZOf}v zRoe@=hmSojmy2vRJLbJK&2En2l9$P3C<>^lYZetX&n$MkDe2XnF?4hyCRsx?()_ICGepH~=MWl7K!SW#iup(oS7zBl5^J<~wN>z+sTTO>4} zQ?YHnasDF?iz;#PJ-1)0J2LB)4YYa@B zolfT{Ezs?D5B@!Xa5#)dQ+yZ956qZV=&0~$G9c~m`SqXUA{-4;R2(B1@=u5)07VF8 z-SWbwJzYcD`s=%G)0+g;{yN0mngGJbGU0Usc>7b}PnJXhoH!*qyEknHcC_Ui`}#(z z-9iXFUN_N#FhwOX!jT}Q1%A3Z+XuUz;gjBham(lPjp&+uz99{4`2JupNKb0mXBt!m zx5q^&9H6i`MmQS8?RJfy`(hkemgMSF%N)Jg{2bZ;#T84!jy97G2MjX0g5Rl8+nq}R zp<^b>*Pm)~GKumzwUVIAvEeI!J?OVgi%e$huFIZNxd?{*L<8)W(x6LY5c2t|Sv7DsJzB0)S}_v^}bHdGJEiL=9n5~9t?%=Y2O@}{IEDe*Wo z>Uwesj*~inCSVTN$ps@PRyr~Ob)GNH%)T{5td~QO4Pvq0f`#u##B}1 z)yuMQ}Bgvbh2$t7`T3Y+Eqn4-)52Wse_4PadCJl5Px}px;2*_vdKva9IB~f;5d6|tbzl0EC1Q7uFLvln6 zKdvYWilT7eJ$JHho~>kf{SL~j0;rmTd6;fq)fCDqe7HRt*$KogPA&E#A!1lE4fa%f50xogb1c&26Q-rVB=iYMXi=7_BFfoRuU`eHP z%Bus!3fu_nZsphSoz7qGy<<#`>`*I(5MqQuR#jDYY}>|?ssN#gld=lm;5q@Xf3&11 z3d9QCRM!L$LeSBA_;GZgO4Y^o+m+1g?O-R>nif?#p~68=CQEh1Q2=xu%Y?TOz~fMy zM>{^12JP%Af}{TcKuy%|dakjv)Q}>Z%jVpg-(KyPO`90HMYPC`FX*5zp_54)%qa6? z7{(|?z~yocDi?~PplRBm>h5qjSpVdcoV6fA<)`;){dSirz-F| z1d-Q~m%K9MtpV8FlxaD1oNDjS{Ni$}#ERUMSNkc5ILKyg-_U?$IUJ5bjSpUn*{ zxRdVF>FylqPm1~!lF6i<#;5-^4+d>R=zu9H91aiu?Ql47I-RsMAEdf?SpJD4EK^LS zbQ~|77$Q@#w?QYKy@5v2zscRMlS^wC`n0t@MCLoncp{goQ zr*lxE+-|o`RBJmGg|ttCTqLkjbCjR`|&f-we%F$064?Xf*J!T6itvCZI_W$*-#KWsi52HQ9c z@PUV5Gk{HoCNxb)Islz2=crN+FP!=DB(+qzq^frBwNzS5s`uX2JMZ3m?z!jeol^7e zm_^`7YLvOyKrwTaU*s$$aLC#uJ<29ctq;`~*5OIJt}ZLXx+)Q>PSD957Q;yc9}Vae z$ec2$bK7yCKTygTBTdyAa_Sigz+hh-f#D>aZW&8Ttk}PAAGUwx_U!*;=BsElnq5o9 z<8cUr0EfeYWHJeX8IU+fU^t19L32orhE>EO8YG#4a|Xj8q>_b6RAd|zi_9ee&<)Br zlmx^zN&>K<*rCs56@maTzDIG6Y1Pw4qJ*|lJ#WI51O_6r%J6XC)J6h;I=8I?0OA^A znmV})tf~e^hZ6_|Qn+!87mq*so9rT2(=@26ic~5EK`@i|qtPgAHXCGF#*Q63aQ<=+ z+FL>x9*pNXJVd5(5^=p?BLT?D>uILS4XmP&*HtY7LnNhT=+lZbl-nZ4fr(KGkCn~1 zpzR47*wdw;XXAKr9IWHc8A96}l`OLs2zA<_hIipzuuDvjoO8k0RKklPHo2 z4B*TZF3Fv%7z1qc)YD{vMKXq(O)ZsX0BR};S(c$`T6Xc9>0P2ILKHK1m` zlP#wf(MNmCM17kmWC@i#3h?1zER&@13Q)MAHZ+%5yE!v82FD}gTzC|Q^))8h`_eK2 zXH|(4gSQ3RO%`}6rWy>uB>PAcPkQcRH(q)1c_@lvDrOs*-e?Y+vMgio-n}@x$+h6p zv3Z8-ibw!plZ5Qk;Z^|y7yRfrc6Eg;0D6h&BcK37rE?`NUti_UxS@Ff02b3pL_t*2 z0AR8nH4r|f;wcNJG>;-N92?a0hDip%HWPiy6i{`NqAAl*;{$6Js{nDF+u2m6AQm3W z?m1-)lyNF-HXHu?(O=`^t7abdWLZGLPZ&-)5&&?lC+1+lh|40?0Orz$5hXi+6%FdR4> zgl9*sJsA|c2q7H|=o*NR8AQCsWy=OA8P|vZcyhD}Cb8o0GoKe?sB70(I!-DQ69*?D zSDy=^e&QM}w7>XK`=Cd0_8US~dTS}(emG?c7SzlXKXGc$01c|g# zTZ3OgcTYPcNy?@;bI#G--HkPkE>zb$5DBNCr3|PlL6R8kP8oKG2vOqDbpvCglS;YI z8o7qL{|}u`0HIR5bqN5BCDgV(z2Rj5k^p|3d)!=CI^XtK#nD=~><=ZVC<#`;yuqTt zu(nb_e}v2-E4};s!NAly@^Muk22kNu1RFq;E>6crQ>du6p|-&ZHDw?cRUwKTtD8Is z27@RsF9+uw4u=DOeD3$SXqC;R^07~TOUM9vyP|oN1#^W}5wZ*2XiQ7B_Q%TYlDLNf zX9K`%?E`&D&1eMhOCrkCgz`5JkRm2zr`a;FW!NTdVkyT=7@Nz8j3vkE` zM+dcOvzj_XvG(+F3NUe79RR|?RJJEcGKbr1f!!(L)+P>CX^ry&7z=sCUlJH7>pRu46`({b;Q8IUTs21YCKv3(Nu{|2rx2401(8<=azxK zDB|N1OfFsTN440EC!hE&s;a6G3Wea540x2uNgD~k&_EpFS!#1k1=Io6vxjlXrI+E}|M*v2yhK4qOAv}( zf?_j=Kf@rXDII~~6jH@#j;G;tOIc<#O*4$A-s!IwIXmuGEX|L7(0RxpS_k0wMD*lj z$Ioae#@L+#V>kWXxwS)13$N@B8j~(^$HOYIY_{ioBBtttI#JU_OI-oLG}26Y-;uD2 zNcdDB)}sjc%grUY;d7tH*DkKYXEv6Zi6mha(eU&tHHE@ca!PaLwF4tvDb1)~>T~wJ z+}7Wh(DVxc8~||BRL4(iD8|?=9;^8E?_5xy@Y#gQp@@O@(b+v+PRqg=sx=TfDGSWy zwK6XeL6i!M{9eDshrihDThJ(Z28vzGhK-1TJAUzz0zelzXWgOr`7jB-E+?YUROguV zS2D&{1Gs;6xxMOZo2tAPkqN_51IOneCS2y=7@5MoE(Rw95#!2L?k*x)3?MJje>#XF zpOAQRI*bGW-kjdl+8Z5hABdH4#(F*&3T}X@S$ruGP0KHy_bVA=8v)!>XBR1K3P`Kv@Uy$mYShGLA>Y^`y9yzkO%(|%D!D|4sRw=iD zZ8{`k8X6b7!8t>4G&y6W&gqij@mbK-Hi6!CPVkqXQq74(wdB(~`PA3u5nUH+DIpc^@3PfGEfbIO=4JEB_f<8=mubyf$pGzkphgQ8LW>u`y9FY&h0Fp@^r4{T338weWjzyC6)ArOy{AHd8VO6Kut~!f~uQt{cH%W zCd2FPgKv19RR1rZtZdAD<}39=nu9Z8cS@+L&0!c*btK~kbe$kc9I};Xw@2bJ4c+a< z^`^QeCoEReiHO9sRMm~Ia!sd`09FyvoW|1$Rq>3mn*e-&iBC!0w64PKwTj*o!}^?A zSr`LK?HnQp;u;}5iRj{NF&a;`{{C1`&C_3LPS_>50Mjk^m(5Q3EEnZfHn=>~OGzYR z8hVNh7rERr%BxSQ;N19w_fJ0cPX8hRpPg#0G#w_TFGy34-UQ%V)h>&4-Rd%UEOL40 zm?6%!yk;Iij~wnea@k6k<<|3kOEMpNye?Rkne`CIgL8)3CP#YnZzAF`4ZWR_q9Wv? z$Wh07hemZ#Zh8WtcF>VYO%+84T|13dOek?ysAHG-Ut)I9_?Au8*BZp^35}4{%oMP2l8S!13PpD) ze)#3q{zbpKsWDXPYJQd{@WY}Vhq6Y@lr!`0ZS7)Swzl$ zd*!0am42ILQhmbHiH>0Oz#Hv@4Zrw&W2!30NP;N9=l2#x8?aE|oS~+{k@fO)o#?`E z?&{umGFS`XdLnwPsPh8=5hXQ)KL7lQzV4w|@_6yhE*uB?$KHOueQ?=twm0dyMq-S? zTjD+=jl>vGS!2r@2>|}j&ptevHqvv&8VMNZ^)bfE1kN7X)>PhD=}=Y{d-AzJCrZB3 zIfY$BqfXT!@aL3|j)l~C(_g*L5-CH5DKGT=d|hi=zCOm7 z6~Gq-&TeY**khZjeQPZOpWw#N6{GP~+gqJOJ)`l|#_e1EoqxW;)5M7K9t262;PyDD z+`au%f-#`1GJm*>#x!-^?eC0r564RZ{4Ejva9*!DoAnoCOakzi4EVa=ZV9d|b=&J) zwiP1J7nz(6!KB*t;ZUfpZ7k|aX!;UaWGA2ea`Pe!r#!V`LEvz>>`>+qa6O$6MUKi^ zdmi~nAef4+x$UjgNJ#YnxRZ$PneXe)nE{hAwh6#z7;u$U6ykMmTiB<_7D?cu#JMO5 zOfm>G8czj>V@Y)^p-Q2oTB#E`06Yiae*nA;K)-gK_xZ1W%y$tHF~%6|4h43HZO)Wx zrwfWrL}iT)LQYb_O`dBSW>sZ+*_Jb2&EVlOI6wg;4GN+QmbIP`_c3(mVOTd zcniQTqI|tF##r^ndi!(tTwA-e)aO*@lf7#`A&ML&WmdSy1#{ncf28Z`@9nMBbkbn1 zm43HSZCG&Wycrw7Se?k(FSo7s3E%xx(`Kv0XFZ%QG&pA{@mt{W##3H^YX zm~yx^G;rlV?XixARX2ba0NhMO0}I}!Gr>?9FvcziaGxS`v7ItcSvx!B-kBdo}iej8pgA`20Cv4*@q>gVbyIBdE)NRE$X>& zxwrD{JKsA%L?2s-Hcf^FEQc-(M5HmsIzE4X)zQD$T2b=dCtK=2^`nFJey6nWmP=~2 zYc`Z_-&B-k9{1TQld*VTt!>T191k>hnq)o2t2rr!1G=98Z}yR4B6 zYSuw2(pkEv3um06(kNTk=tPxEF8lcxmLq9U^ubrVo!@(=t@-X}+6Grv+fQ7zu2jBk zjc=*PCQf<7IA@S80%Qx%dzW&CH4qSvs>zc*vB2wxM#8__HDKM}609M@;%cvD=#DGv zL)%vS>S53#N+*wyS(kTO4Ryv1eE`51`@ti(t^V}tN_$>NiE-#VPXs%E@=}+0q$^Sj zz_3~P@$*;sqJOrg#InBG-*?zbM9m9*FETh&dsJ62Hjz7r z3<&2myBp8>svE?h-uS?g{==OS{e=TV#tR3A8UW08>sJBbfmz~m7-p3FW1rYinz-jP z&5MXKz2%gVWQzcQnH6@Ygb&~89RW~Wiqaf$rW&gKx9<(~e(s{`#Y6pZWORCS!3dJe zDrx{8C!)L4)%x!A%aeY&bKac*hF}_{13(86ovNK>?0?*fxRl7Os<$X2tQG;XmBV5Y zvN;d{&^Z*>0UTSP-##hMR734P(h>I9>>}!#ok+$tBohXb2`#N}Xc}%Xs9In~Kah5l z$I`Ek0q8=Xf1cVh0Sj)Z zbXe$a02=`4rEdAaC%2Tpdg+Ez&&O7Ho4gLG0zk#|XE}vrnsst-Yq$r%_X}*31YjwE zH83@x;s9cixIPRHm#P|*E~wu-7C5$LnYaJab$;B05?~+oo*uLan11W19i|5Wu;LES&htm34i0T+_I`%p*@=8>H`V3AX(3skYu1_6{|qG-C*6;q-gO{N8N)7s61Cv1F60y4~=`%5CqE5v1tT>{`ymm(_+Q6I_qPOBEv~YE^x*BQe4Cegm(FyJL|n&Ee=M&OFrpg4Z#;Y~u=`{%0pR0A z)S7OO1fct~=adKUzF~2Vzr?DT>mpz>hA=WrvPC`(dGSn$MK$zy$B@#E)Tiz}INClG z|1uFhJM)oKV<8N6J%C5|+`Ty_b7r&IM0iWBP;95F*Pnd5fB!cg{6H89t1H$uyLSKL zjx{yQYaR7doimlv5s&F61whWz9tx%~GMLCt{tUp2AB?vA-J{1H0InjUz3IBI;h%qb zMc<|dS3~Z3IcH#;fx%4OHgw9yB3y16r4>q^!LIq|eLF8&;qhF&%2(~M2yRfi4%igZ z0|PAyLJx{(GnDag)metfI}Xe$Zo!%YT&|^B@T~GR1{f|xX9J(esr*< zr9ZY|S&d`YQ+KX!XsobRPIaziLdWQE5|NM!fFOtrUY`|iubj=8H4MVYU?PvBXe^-x zF1}|UG@Y&_qFs!!p8@#Nc}u;mSD#%(G{0*~L1))#`Ujc84q~oRMUJ zY0;z}d+^on!T)-*x1ET#6?I=1XpTA;z<{+v|-B*Z5XUb$lYGVRSfw zSX4v%NTToGp6y87zkOL93^Os4a|WkdhTAJ=(}-gc4gI|_Q_L7Z$7rJO>i>AZort!k z!%JTPa2|lw09F970#ISD-G%^k0oV=Ty^pUiz33ZP)_Xl6vg6 z2R_*S_>SInL~z4g{2l{P1)w_p?t#f2@fLu4r=upA4HnYtV=x<}IZt7F^$@411;BlDAj6Lb|$N)YI;8wp=UV6?F_sG^I z9;~c!ILh6!14K+G=xsys;NL%T+}aaJK0!pc6xe14;MM0rpx^YM@P2Zvrmbo^o!=p9iPyi(`2Y;&RyzV{lNC64KU29mjHk>J(kjAePgNc z;m&aEz85=vBVpAGbL-(YBHA&{IoASM2cQ;a7^)6H4S)d5fayyBo-8(}fiYGC;4Xo) z8^3f>b?5dAs;aF5w+kYN%Pqs^5HZvr$H9(p_jUIjdWVRvE9QDrw&OGz>Usc=2%PDP z#FG}8r=khnl2EfDq6mN;Fga9TC8EO%-l#c(F}4`M?EtPTbIa_Sjin=(toGZzcG1EZ z!$Yri2Oitm-$F!JEXegw*A6dZY#V?-1JDR_0v~`GeEkT(d*c_WXDZU`vdaKm53@(t l1L%eMp#8&ql-Nv${{{57GN5%uv1$MS002ovPDHLkV1jO_k!b(` literal 0 HcmV?d00001 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc index 507b4c877..218840334 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc +++ b/ground/openpilotgcs/src/plugins/setupwizard/wizardResources.qrc @@ -29,6 +29,7 @@ resources/bttn-calculate-up.png resources/bttn-turbo-down.png resources/bttn-turbo-up.png + resources/wizard.png resources/multirotor-shapes.svg resources/fixedwing-shapes.svg From 741efd9281950ce024af688f017b708fcc842816 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 01:57:15 +1000 Subject: [PATCH 143/718] Add image --- .../src/plugins/setupwizard/pages/opstartpage.ui | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui index 589be9eb9..2f161f29f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/opstartpage.ui @@ -39,16 +39,18 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </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;">Welcome to the OpenPilot Setup Wizard</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;"><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;">This wizard will guide you through the basic steps required to setup your OpenPilot controller for the first time. You will be asked questions about your platform (multirotor/heli/fixed-wing) which this wizard will use to configure your controller for its first flight. </span></p> +<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;">Welcome to the OpenPilot Setup Wizard</span><img src=":/setupwizard/resources/wizard.png" style="float: right;" /></p> +<p align="center" 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 align="right" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><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;">This wizard will guide you through the basic steps required to setup your OpenPilot controller for the first time. You will be asked questions about your platform (multirotor/heli/fixed-wing) which this wizard will use to configure your controller for its first flight.</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;">This wizard does not configure all of the advanced settings available in the GCS Configuration. All basic and advanced configuration parameters can be modified later by using the GCS Configuration plugin.</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="-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 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; color:#ff0000;">WARNING: YOU MUST REMOVE ALL PROPELLERS </span></p> <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; color:#ff0000;">FROM THE VEHICLE BEFORE PROCEEDING!</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:'Lucida Grande'; font-size:13pt;"><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;">Disregarding this warning puts you at</span><span style=" font-size:10pt; font-weight:600; color:#000000;"> risk of very serious injury</span><span style=" font-size:10pt;">!</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;">Disregarding this warning puts you at</span><span style=" font-size:10pt; font-weight:600; color:#000000;"> risk of injury</span><span style=" font-size:10pt;">!</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;">Now that your props are removed we can get started. Ready?</span></p></body></html> From 454ea624ba0838f4c93d0941cf3b5b865314ba95 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Sat, 16 Aug 2014 19:57:57 +0200 Subject: [PATCH 144/718] OP-1450 Turn on high resolution support for OSX --- ground/openpilotgcs/src/app/Info.plist | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ground/openpilotgcs/src/app/Info.plist b/ground/openpilotgcs/src/app/Info.plist index fd048113f..1ada02aee 100644 --- a/ground/openpilotgcs/src/app/Info.plist +++ b/ground/openpilotgcs/src/app/Info.plist @@ -18,5 +18,9 @@ 1.3.1 CFBundleShortVersionString 1.3.1 + NSPrincipalClass + NSApplication + NSHighResolutionCapable + True From 4c242fa2e05f724913e2e9a037ead65713224129 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 04:01:21 +1000 Subject: [PATCH 145/718] Wording of FW wizard page --- .../setupwizard/pages/fixedwingpage.ui | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui index b1c5f253f..0ae63ca99 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui @@ -16,26 +16,26 @@ - - <!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; } + + <!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:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot fixedwing configuration</span></p> +<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 fixed-wing 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: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;">This part of the wizard will set up the OpenPilot controller for use with a flying platform utilizing multiple rotors. The wizard supports the most common types of fixedwings. Other variants of fixedwings can be configured by using custom configuration options in the Configuration plugin in the GCS.</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;">This part of the wizard will set up the OpenPilot controller for use with a fixed-wing flying aircraft utilizing servos. The wizard supports the most common types of fixed-wing aircraft, other variants of fixed-wing aircraft can be configured by using custom configuration options in the Configuration plugin in the GCS.</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;">Please select the type of fixedwing you want to create a configuration for below:</span></p></body></html> - - +<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;">Please select the type of fixed-wing you want to create a configuration for below:</span></p></body></html> + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - true - - - - + + + true + + + + 4 @@ -63,7 +63,7 @@ - FixedWing type: + Fixed-wing type: From 3a4b27df5ded95cbcffeb4197f8bbc6a7fcf4c5c Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 04:15:11 +1000 Subject: [PATCH 146/718] Minor formatting of all Wiz pages for consitency --- .../setupwizard/pages/biascalibrationpage.ui | 4 ++-- .../plugins/setupwizard/pages/controllerpage.ui | 2 +- .../plugins/setupwizard/pages/fixedwingpage.ui | 2 +- .../src/plugins/setupwizard/pages/inputpage.ui | 2 +- .../src/plugins/setupwizard/pages/multipage.ui | 2 +- .../setupwizard/pages/outputcalibrationpage.ui | 2 +- .../src/plugins/setupwizard/pages/outputpage.ui | 16 ++++++++-------- .../setupwizard/pages/revocalibrationpage.ui | 2 +- .../src/plugins/setupwizard/pages/summarypage.ui | 2 +- .../src/plugins/setupwizard/pages/vehiclepage.ui | 4 ++-- 10 files changed, 19 insertions(+), 19 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui index 3c85d4603..345fc5217 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/biascalibrationpage.ui @@ -21,11 +21,11 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot sensor calibration procedure</span></p> +<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 Sensor Calibration Procedure</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 wizard needs to get information from the controller to determine in which position the vehicle is normally considered to be level. To be able to successfully perform these measurements, you need to place the vehicle on a surface that is as flat and level as possible. Examples of such surfaces could be a table top or the floor. Be careful to ensure that the vehicle really is level, since this step will affect the accelerometer and gyro bias in the controller software.</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;">To perform the calibration, please push the Calculate button and wait for the process to finish. </span></p></body></html> +<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;">To perform the calibration, please push the Calculate button and wait for the process to finish.</span></p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui index 124f54c86..ac74de048 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/controllerpage.ui @@ -33,7 +33,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot board identification</span></p> +<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 Board Identification</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;"><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;">To continue, the wizard needs to determine the configuration required for the type of OpenPilot controller you have. When connected, the wizard will attempt to automatically detect the type of board.</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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui index 0ae63ca99..e4bba0fd7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/fixedwingpage.ui @@ -21,7 +21,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </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 fixed-wing Configuration</span></p> +<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 Fixed-wing 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: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;">This part of the wizard will set up the OpenPilot controller for use with a fixed-wing flying aircraft utilizing servos. The wizard supports the most common types of fixed-wing aircraft, other variants of fixed-wing aircraft can be configured by using custom configuration options in the Configuration plugin in the GCS.</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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui index cf17df324..48a03d7d8 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/inputpage.ui @@ -24,7 +24,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot basic input signal configuration</span></p> +<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 Input Signal 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: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 OpenPilot controller supports many different types of input signals. Please select the type of input that matches your receiver configuration. If you are unsure, just leave the default option selected and continue the wizard.</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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui index 0883ec4d5..092c76187 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/multipage.ui @@ -21,7 +21,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot Multirotor Configuration</span></p> +<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 Multirotor 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: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;">This part of the wizard will set up the OpenPilot controller for use with a flying platform utilizing multiple rotors. The wizard supports the most common types of multirotors. Other variants of multirotors can be configured by using custom configuration options in the Configuration plugin in the GCS.</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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui index efff8d7d5..f80cab6de 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui @@ -27,7 +27,7 @@ Output calibration - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Qt::AlignCenter diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui index 9265d3845..e7f65cc6f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.ui @@ -20,14 +20,14 @@ <!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:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot basic output signal 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:12pt; font-weight:600;"></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 set an optimal configuration of the output signals powering your motors, the wizard needs to know what type of Electronic Speed Controllers (ESCs) you will use and what their capabilities are.</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;"></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;">Please select one of the options below. If you are unsure about the capabilities of your ESCs, just leave the default option selected and continue the wizard.</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;"></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 read more regarding ESC refresh rates, please see </span><a href="http://wiki.openpilot.org/x/HIEu"><span style=" text-decoration: underline; color:#0000ff;">this article</span></a><span style=" font-family:'MS Shell Dlg 2'; font-size:10pt;"> in the OpenPilot Wiki</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 Output Signal 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: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;">To set an optimal configuration of the output signals powering your motors, the wizard needs to know what type of Electronic Speed Controllers (ESCs) you will use and what their capabilities are.</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;">Please select one of the options below. If you are unsure about the capabilities of your ESCs, just leave the default option selected and continue the wizard.</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;">To read more regarding ESC refresh rates, please see </span><a href="http://wiki.openpilot.org/x/HIEu"><span style=" font-family:'Lucida Grande'; font-size:13pt; text-decoration: underline; color:#0000ff;">this article</span></a><span style=" font-size:10pt;"> in the OpenPilot Wiki</span></p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.ui index c68a19a84..173a8f32f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/revocalibrationpage.ui @@ -27,7 +27,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot Revolution controller sensor calibration procedure</span></p> +<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 Revolution Calibration Procedure</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 calibration procedure for the OpenPilot Revolution controller is not yet implemented in this setup wizard. To calibrate your OpenPilot Revolution controller please use the calibration utility found in the configuration plugin after you have finished this wizard.</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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui index 659d46c2b..3e2fe7f7d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/summarypage.ui @@ -21,7 +21,7 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">OpenPilot configuration summary</span></p> +<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> diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui index 34d9531fe..b78313744 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui @@ -21,13 +21,13 @@ <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> -<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:12pt; font-weight:600;">Vehicle type selection</span></p> +<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;">Vehicle Type Selection</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;">To continue, the wizard needs to know what type of vehicle the OpenPilot controller board is going to be used with. This step is crucial since much of the following configuration is unique per vehicle type.</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;"><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;">Go ahead and select the type of vehicle for which you want to create a 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 current version only provides functionality for multirotors.)</span></p></body></html> +<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 current version only provides functionality for Multirotors and Fixed-wing aircraft.)</span></p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop From 760ae118e997245e4139a22d3a034267a8d80240 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 16 Aug 2014 20:48:56 +0200 Subject: [PATCH 147/718] OP-1156 changed yaw math from interval [0..360] to [-180..180] --- flight/modules/PathFollower/pathfollower.c | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 69e3cc7aa..fda14d29f 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -420,12 +420,7 @@ static float updateTailInBearing() TakeOffLocationData t; TakeOffLocationGet(&t); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(p.East - t.East, p.North - t.North)); } /** @@ -437,12 +432,7 @@ static float updateCourseBearing() VelocityStateGet(&v); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(v.East, v.North)); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(v.East, v.North)); } /** @@ -462,12 +452,7 @@ static float updatePathBearing() path_progress(&pathDesired, cur, &progress); // atan2f always returns in between + and - 180 degrees - float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); - // result is in between 0 and 360 degrees - if (yaw < 0.0f) { - yaw += 360.0f; - } - return yaw; + return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0])); } /** From c620c7fc2937d5d054f7ce3130d8d62a37b77424 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 07:19:29 +1000 Subject: [PATCH 148/718] Change message about installing 7zip on Linux, packaging changed in latest LTS --- make/tools.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/make/tools.mk b/make/tools.mk index 913b90901..ccf0da3bd 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -422,7 +422,7 @@ define LINUX_QT_INSTALL_TEMPLATE qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR) $(V1) if ! $(SEVENZIP) >/dev/null 2>&1; then \ - $(ECHO) $(MSG_NOTICE) "Please install the p7zip for your distribution. i.e.: sudo apt-get install p7zip." && \ + $(ECHO) $(MSG_NOTICE) "Please install the p7zip for your distribution. i.e.: sudo apt-get install p7zip-full" && \ exit 1; \ fi $(call DOWNLOAD_TEMPLATE,$(3),$(5),"$(4)") From bc8ccb7be7f7bc30cf7ff9ad2ee68aee7b8cbef5 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 16 Aug 2014 23:59:37 +0200 Subject: [PATCH 149/718] Revert "Revert "Merge remote-tracking branch 'origin/parched/OP-1324_fix_build_when_python_isnt_python2' into next"" This reverts commit 449653f01e4221f104579d47b25e2b0d9664a890. --- ground/openpilotgcs/src/python.pri | 2 +- make/tools.mk | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/python.pri b/ground/openpilotgcs/src/python.pri index 739fc2cc9..62d03795f 100644 --- a/ground/openpilotgcs/src/python.pri +++ b/ground/openpilotgcs/src/python.pri @@ -15,7 +15,7 @@ OPENPILOT_TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) PYTHON = \"$$ROOT_DIR/tools/$$PYTHON_DIR/python\" } else { # not found, hope it's in the path... - PYTHON = \"python\" + PYTHON = $$(PYTHON) } } diff --git a/make/tools.mk b/make/tools.mk index ccf0da3bd..925751d0e 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -690,7 +690,11 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists) else # not installed, hope it's in the path... # $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH) - export PYTHON := python + ifeq ($(findstring Python 2,$(shell python --version)), Python 2) + export PYTHON := python + else + export PYTHON := python2 + endif endif .PHONY: python_version From 9e8beda778d8ca09acbe6591577dd9d39abb1409 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 17 Aug 2014 11:08:02 +1200 Subject: [PATCH 150/718] added quotation marks for PYTHON --- ground/openpilotgcs/src/python.pri | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/python.pri b/ground/openpilotgcs/src/python.pri index 62d03795f..3643adc02 100644 --- a/ground/openpilotgcs/src/python.pri +++ b/ground/openpilotgcs/src/python.pri @@ -15,7 +15,7 @@ OPENPILOT_TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR) PYTHON = \"$$ROOT_DIR/tools/$$PYTHON_DIR/python\" } else { # not found, hope it's in the path... - PYTHON = $$(PYTHON) + PYTHON = \"$$(PYTHON)\" } } From dc191f7523b4efa59fb5af0a448ba00b6856d138 Mon Sep 17 00:00:00 2001 From: James Duley Date: Sun, 17 Aug 2014 20:33:39 +1200 Subject: [PATCH 151/718] changed udev from plugdev to uacces/udev-acl --- package/linux/45-openpilot-permissions.rules | 63 +++++++++++++------- 1 file changed, 41 insertions(+), 22 deletions(-) diff --git a/package/linux/45-openpilot-permissions.rules b/package/linux/45-openpilot-permissions.rules index 9015612a6..fdca37665 100644 --- a/package/linux/45-openpilot-permissions.rules +++ b/package/linux/45-openpilot-permissions.rules @@ -1,22 +1,41 @@ - # OpenPilot openpilot flight control board - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4117", MODE="0664", GROUP="plugdev" - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415a", MODE="0664", GROUP="plugdev" - # OpenPilot coptercontrol flight control board - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415b", MODE="0664", GROUP="plugdev" - # OpenPilot OPLink Mini radio modem board - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415c", MODE="0664", GROUP="plugdev" - # OpenPilot Revolution board - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415e", MODE="0664", GROUP="plugdev" - - # Other OpenPilot reserved pids - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415d", MODE="0664", GROUP="plugdev" - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4194", MODE="0664", GROUP="plugdev" - SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4195", MODE="0664", GROUP="plugdev" - - - # unprogrammed openpilot flight control board - SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5750", MODE="0664", GROUP="plugdev" - # FTDI FT2232C Dual USB-UART/FIFO IC - SUBSYSTEM=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010", MODE="0664", GROUP="plugdev" - # Olimex Ltd. OpenOCD JTAG TINY - SUBSYSTEM=="usb", ATTRS{idVendor}=="15ba", ATTRS{idProduct}=="0004", MODE="0664", GROUP="plugdev" +# Skip this section below if this device is not connected by USB +SUBSYSTEM!="usb", GOTO="op_rules_end" + +# OpenPilot openpilot flight control board +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4117", GOTO="op_rules" +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415a", GOTO="op_rules" + +# OpenPilot coptercontrol flight control board +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415b", GOTO="op_rules" + +# OpenPilot OPLink Mini radio modem board +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415c", GOTO="op_rules" + +# OpenPilot Revolution board +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415e", GOTO="op_rules" + +# Other OpenPilot reserved pids +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="415d", GOTO="op_rules" +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4194", GOTO="op_rules" +SUBSYSTEM=="usb", ATTRS{idVendor}=="20a0", ATTRS{idProduct}=="4195", GOTO="op_rules" + + +# unprogrammed openpilot flight control board +SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5750", GOTO="op_rules" +# FTDI FT2232C Dual USB-UART/FIFO IC +SUBSYSTEM=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010", GOTO="op_rules" +# Olimex Ltd. OpenOCD JTAG TINY +SUBSYSTEM=="usb", ATTRS{idVendor}=="15ba", ATTRS{idProduct}=="0004", GOTO="op_rules" + +GOTO="op_rules_end" + +LABEL="op_rules" +# Allow any seated user to access the board. +# uaccess: modern ACL-enabled udev +# udev-acl: for Ubuntu 12.10 and older +TAG+="uaccess", TAG+="udev-acl" + +# Grant members of the "plugdev" group access to receiver (useful for SSH users) +#MODE="0664", GROUP="plugdev" + +LABEL="op_rules_end" From 3fc991fe2c56463099eb2932698e6792a903c584 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 20:31:57 +1000 Subject: [PATCH 152/718] Add the outputpage for fixed wing support --- .../setupwizard/pages/outputpagefixedwing.cpp | 54 +++++++ .../setupwizard/pages/outputpagefixedwing.h | 49 ++++++ .../setupwizard/pages/outputpagefixedwing.ui | 153 ++++++++++++++++++ .../src/plugins/setupwizard/setupwizard.cpp | 7 +- .../src/plugins/setupwizard/setupwizard.h | 7 +- .../src/plugins/setupwizard/setupwizard.pro | 3 + 6 files changed, 269 insertions(+), 4 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp new file mode 100644 index 000000000..66db90bdf --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp @@ -0,0 +1,54 @@ +/** + ****************************************************************************** + * + * @file outputpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputPage + * @{ + * @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 "outputpagefixedwing.h" +#include "ui_outputpagefixedwing.h" +#include "setupwizard.h" + +OutputPageFixedwing::OutputPageFixedwing(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + + ui(new Ui::OutputPageFixedwing) +{ + ui->setupUi(this); +} + +OutputPageFixedwing::~OutputPageFixedwing() +{ + delete ui; +} + +bool OutputPageFixedwing::validatePage() +{ + if (ui->rapidESCButton->isChecked()) { + getWizard()->setESCType(SetupWizard::ESC_RAPID); + } else { + getWizard()->setESCType(SetupWizard::ESC_LEGACY); + } + + return true; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h new file mode 100644 index 000000000..d3d99b0fa --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file outputpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup OutputPage + * @{ + * @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 OUTPUTPAGEFIXEDWING_H +#define OUTPUTPAGEFIXEDWING_H + +#include "abstractwizardpage.h" + +namespace Ui { +class OutputPageFixedwing; +} + +class OutputPageFixedwing : public AbstractWizardPage { + Q_OBJECT + +public: + explicit OutputPageFixedwing(SetupWizard *wizard, QWidget *parent = 0); + ~OutputPage(); + bool validatePage(); + +private: + Ui::OutputPageFixedwing *ui; +}; + +#endif // OUTPUTPAGEFIXEDWING_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui new file mode 100644 index 000000000..bb7132888 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui @@ -0,0 +1,153 @@ + + + OutputPageFixedwing + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + + <!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:'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 Output Signal 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: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;">To set an optimal configuration of the output signals powering your servos, the wizard needs to know what type of servos you will use and what their capabilities are.</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;">Please select one of the options below. If you are unsure about the capabilities of your servos, just leave the default option selected and continue the wizard.</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></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + true + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + 10 + + + + Standard ESC 50Hz + + + QToolButton { border: none } + + + Standard Servos + + + + :/setupwizard/resources/bttn-ESC-up.png + :/setupwizard/resources/bttn-ESC-down.png:/setupwizard/resources/bttn-ESC-up.png + + + + 200 + 100 + + + + true + + + false + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + + + 10 + + + + Turbo PWM ESC 400Hz + + + QToolButton { border: none } + + + Digital Servos + + + + :/setupwizard/resources/bttn-turbo-up.png + :/setupwizard/resources/bttn-turbo-down.png:/setupwizard/resources/bttn-turbo-up.png + + + + 200 + 100 + + + + true + + + true + + + true + + + Qt::ToolButtonTextUnderIcon + + + true + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index eccfd2ce2..2ee8bf039 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -36,6 +36,7 @@ #include "pages/surfacepage.h" #include "pages/inputpage.h" #include "pages/outputpage.h" +#include "pages/outputpagefixedwing.h" #include "pages/biascalibrationpage.h" #include "pages/summarypage.h" #include "pages/savepage.h" @@ -113,7 +114,7 @@ int SetupWizard::nextId() const return PAGE_OUTPUT; case PAGE_FIXEDWING: - return PAGE_OUTPUT; + return PAGE_OUTPUT_FIXEDWING; case PAGE_INPUT: if (isRestartNeeded()) { @@ -128,6 +129,9 @@ int SetupWizard::nextId() const case PAGE_OUTPUT: return PAGE_SUMMARY; + case PAGE_OUTPUT_FIXEDWING: + return PAGE_SUMMARY; + case PAGE_BIAS_CALIBRATION: return PAGE_OUTPUT_CALIBRATION; @@ -319,6 +323,7 @@ void SetupWizard::createPages() setPage(PAGE_SURFACE, new SurfacePage(this)); setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this)); + setPage(PAGE_OUTPUT_FIXEDWING, new OutputPageFixedwing(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); // setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index a908ac989..434856607 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -156,9 +156,10 @@ private slots: void pageChanged(int currId); private: enum { PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, - PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_BIAS_CALIBRATION, - PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, PAGE_SAVE, PAGE_SUMMARY, - PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, PAGE_UPDATE }; + PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_OUTPUT_FIXEDWING, + PAGE_BIAS_CALIBRATION,PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, + PAGE_SAVE, PAGE_SUMMARY,PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, + PAGE_UPDATE }; void createPages(); bool saveHardwareSettings() const; bool canAutoUpdate() const; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro index 366b7eae9..51a98c880 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -23,6 +23,7 @@ HEADERS += setupwizardplugin.h \ pages/surfacepage.h \ pages/abstractwizardpage.h \ pages/outputpage.h \ + pages/outputpagefixedwing.h \ pages/inputpage.h \ pages/summarypage.h \ vehicleconfigurationsource.h \ @@ -50,6 +51,7 @@ SOURCES += setupwizardplugin.cpp \ pages/surfacepage.cpp \ pages/abstractwizardpage.cpp \ pages/outputpage.cpp \ + pages/outputpagefixedwing.cpp \ pages/inputpage.cpp \ pages/summarypage.cpp \ vehicleconfigurationsource.cpp \ @@ -77,6 +79,7 @@ FORMS += \ pages/helipage.ui \ pages/surfacepage.ui \ pages/outputpage.ui \ + pages/outputpagefixedwing.ui \ pages/inputpage.ui \ pages/summarypage.ui \ connectiondiagram.ui \ From ea1e95db7ba94b69136f5ab12f36d4f6c9fb1c4a Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 22:55:11 +1000 Subject: [PATCH 153/718] Minor refactroring of a type in a class, change ESC_TYPE and geter / serter to be actuator type as we are dealing with servos. Add servos to enum, remove hard coded min / max for now. --- .../plugins/setupwizard/pages/outputpage.cpp | 4 +- .../setupwizard/pages/outputpagefixedwing.cpp | 6 +-- .../setupwizard/pages/outputpagefixedwing.h | 2 +- .../setupwizard/pages/outputpagefixedwing.ui | 2 +- .../src/plugins/setupwizard/setupwizard.cpp | 11 ++++- .../src/plugins/setupwizard/setupwizard.h | 6 +-- .../vehicleconfigurationhelper.cpp | 47 ++++++++++++++----- .../setupwizard/vehicleconfigurationhelper.h | 5 +- .../setupwizard/vehicleconfigurationsource.h | 4 +- 9 files changed, 59 insertions(+), 28 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp index b57237f3f..b00c6cfa3 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpage.cpp @@ -45,9 +45,9 @@ OutputPage::~OutputPage() bool OutputPage::validatePage() { if (ui->rapidESCButton->isChecked()) { - getWizard()->setESCType(SetupWizard::ESC_RAPID); + getWizard()->setActuatorType(SetupWizard::ESC_RAPID); } else { - getWizard()->setESCType(SetupWizard::ESC_LEGACY); + getWizard()->setActuatorType(SetupWizard::ESC_LEGACY); } return true; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp index 66db90bdf..b635a5e55 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp @@ -44,10 +44,10 @@ OutputPageFixedwing::~OutputPageFixedwing() bool OutputPageFixedwing::validatePage() { - if (ui->rapidESCButton->isChecked()) { - getWizard()->setESCType(SetupWizard::ESC_RAPID); + if (ui->ServoTypeButton->isChecked()) { + getWizard()->setActuatorType(SetupWizard::SERVO_DIGITAL); } else { - getWizard()->setESCType(SetupWizard::ESC_LEGACY); + getWizard()->setActuatorType(SetupWizard::SERVO_LEGACY); } return true; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h index d3d99b0fa..f3494b85b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h @@ -39,7 +39,7 @@ class OutputPageFixedwing : public AbstractWizardPage { public: explicit OutputPageFixedwing(SetupWizard *wizard, QWidget *parent = 0); - ~OutputPage(); + ~OutputPageFixedwing(); bool validatePage(); private: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui index bb7132888..4d342ba47 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui @@ -99,7 +99,7 @@ p, li { white-space: pre-wrap; } - + 10 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index 2ee8bf039..c40e0e253 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -291,14 +291,21 @@ QString SetupWizard::getSummaryText() } summary.append("
"); - summary.append("").append(tr("ESC type: ")).append(""); - switch (getESCType()) { + summary.append("").append(tr("Actuator type: ")).append(""); + switch (getActuatorType()) { case ESC_LEGACY: summary.append(tr("Legacy ESC (50 Hz)")); break; case ESC_RAPID: summary.append(tr("Rapid ESC (400 Hz)")); break; + case SERVO_LEGACY: + summary.append(tr("Legacy Servos (50 Hz)")); + break; + case SERVO_DIGITAL: + summary.append(tr("Digital Servos (333 Hz)")); + break; + default: summary.append(tr("Unknown")); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index 434856607..c941561af 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -78,11 +78,11 @@ public: return m_inputType; } - void setESCType(SetupWizard::ESC_TYPE type) + void setActuatorType(SetupWizard::ACTUATOR_TYPE type) { m_escType = type; } - SetupWizard::ESC_TYPE getESCType() const + SetupWizard::ACTUATOR_TYPE getActuatorType() const { return m_escType; } @@ -168,7 +168,7 @@ private: VEHICLE_TYPE m_vehicleType; VEHICLE_SUB_TYPE m_vehicleSubType; INPUT_TYPE m_inputType; - ESC_TYPE m_escType; + ACTUATOR_TYPE m_escType; GPS_SETTING m_gpsSetting; RADIO_SETTING m_radioSetting; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 930d6e928..dbca5b044 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -42,6 +42,8 @@ const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; +const qint16 VehicleConfigurationHelper::LEGACY_SERVO_FREQUENCE = 50; +const qint16 VehicleConfigurationHelper::DIGITAL_SERVO_FREQUENCE = 333; VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) : m_configSource(configSource), m_uavoManager(0), @@ -68,7 +70,7 @@ bool VehicleConfigurationHelper::setupVehicle(bool save) applyHardwareConfiguration(); applyVehicleConfiguration(); applyActuatorConfiguration(); - applyFlighModeConfiguration(); + applyFlightModeConfiguration(); if (save) { applySensorBiasConfiguration(); @@ -237,8 +239,10 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() break; } case VehicleConfigurationSource::VEHICLE_HELI: + // TODO: Implement settings for Helis + break; case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + // TODO: Implement settings for Surface break; default: break; @@ -270,7 +274,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() } qint16 updateFrequence = LEGACY_ESC_FREQUENCE; - switch (m_configSource->getESCType()) { + switch (m_configSource->getActuatorType()) { case VehicleConfigurationSource::ESC_LEGACY: updateFrequence = LEGACY_ESC_FREQUENCE; break; @@ -322,18 +326,31 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() { ActuatorSettings::DataFields data = actSettings->getData(); - qDebug() << "Override center, min and max pulses for fixed wing servos\n"; - // move all but first chan to 1500 center pluse QList actuatorSettings = m_configSource->getActuatorSettings(); for (quint16 i = 1; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelAddr[i] = i; - data.ChannelMin[i] = 554; // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, - // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU - data.ChannelNeutral[i] = 1500; - data.ChannelMax[i] = 2400; // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg + data.ChannelMin[i] = actuatorSettings[i].channelMin; + data.ChannelNeutral[i] = actuatorSettings[i].channelNeutral; + data.ChannelMax[i] = actuatorSettings[i].channelMax; } - qDebug() << "Save Fixed Wing Actuator Data\n"; + + for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + data.ChannelUpdateFreq[i] = LEGACY_SERVO_FREQUENCE; + } + + qint16 updateFrequence = LEGACY_SERVO_FREQUENCE; + switch (m_configSource->getActuatorType()) { + case VehicleConfigurationSource::SERVO_LEGACY: + updateFrequence = LEGACY_SERVO_FREQUENCE; + break; + case VehicleConfigurationSource::SERVO_DIGITAL: + updateFrequence = DIGITAL_SERVO_FREQUENCE; + break; + default: + break; + } + actSettings->setData(data); addModifiedObject(actSettings, tr("Writing actuator settings")); @@ -389,15 +406,19 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() } case VehicleConfigurationSource::VEHICLE_HELI: - case VehicleConfigurationSource::VEHICLE_SURFACE: - // TODO: Implement settings for other vehicle types? + // TODO: Implement settings for Heli vehicle types break; + + case VehicleConfigurationSource::VEHICLE_SURFACE: + // TODO: Implement settings for ground vehicle types + break; + default: break; } } -void VehicleConfigurationHelper::applyFlighModeConfiguration() +void VehicleConfigurationHelper::applyFlightModeConfiguration() { FlightModeSettings *modeSettings = FlightModeSettings::GetInstance(m_uavoManager); ManualControlSettings *controlSettings = ManualControlSettings::GetInstance(m_uavoManager); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index 7829ac039..b2e525483 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -59,6 +59,9 @@ public: bool setupHardwareSettings(bool save = true); static const qint16 LEGACY_ESC_FREQUENCE; static const qint16 RAPID_ESC_FREQUENCE; + static const qint16 LEGACY_SERVO_FREQUENCE; + static const qint16 DIGITAL_SERVO_FREQUENCE; + signals: void saveProgress(int total, int current, QString description); @@ -79,7 +82,7 @@ private: void applyHardwareConfiguration(); void applyVehicleConfiguration(); void applyActuatorConfiguration(); - void applyFlighModeConfiguration(); + void applyFlightModeConfiguration(); void applySensorBiasConfiguration(); void applyStabilizationConfiguration(); void applyManualControlDefaults(); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 733518ee2..659da60fb 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -61,7 +61,7 @@ public: MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, MULTI_ROTOR_OCTO_X, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, FIXED_WING_VTAIL, FIXED_WING_ELEVON, HELI_CCPM }; - enum ESC_TYPE { ESC_RAPID, ESC_LEGACY, ESC_UNKNOWN }; + enum ACTUATOR_TYPE { ESC_RAPID, ESC_LEGACY, SERVO_LEGACY, SERVO_DIGITAL, ESC_UNKNOWN }; enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; @@ -71,7 +71,7 @@ public: virtual VehicleConfigurationSource::VEHICLE_TYPE getVehicleType() const = 0; virtual VehicleConfigurationSource::VEHICLE_SUB_TYPE getVehicleSubType() const = 0; virtual VehicleConfigurationSource::INPUT_TYPE getInputType() const = 0; - virtual VehicleConfigurationSource::ESC_TYPE getESCType() const = 0; + virtual VehicleConfigurationSource::ACTUATOR_TYPE getActuatorType() const = 0; virtual VehicleConfigurationSource::GPS_SETTING getGPSSetting() const = 0; virtual VehicleConfigurationSource::RADIO_SETTING getRadioSetting() const = 0; From 2b6629e03a03043a54087756c57752b6fc0f2cbd Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Sun, 17 Aug 2014 23:06:01 +1000 Subject: [PATCH 154/718] Change wording, standard servos to analog servos --- .../src/plugins/setupwizard/pages/outputpagefixedwing.ui | 2 +- ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui index 4d342ba47..cf32527b4 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui @@ -68,7 +68,7 @@ p, li { white-space: pre-wrap; } QToolButton { border: none } - Standard Servos + Analog Servos diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index c40e0e253..dd82b7cf5 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -300,7 +300,7 @@ QString SetupWizard::getSummaryText() summary.append(tr("Rapid ESC (400 Hz)")); break; case SERVO_LEGACY: - summary.append(tr("Legacy Servos (50 Hz)")); + summary.append(tr("Analog Servos (50 Hz)")); break; case SERVO_DIGITAL: summary.append(tr("Digital Servos (333 Hz)")); From a46d5316ec4a65e81c7930482d624420845e7a5c Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Mon, 18 Aug 2014 02:51:49 +1000 Subject: [PATCH 155/718] Tooltips --- .../src/plugins/setupwizard/pages/outputpagefixedwing.ui | 4 ++-- .../openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui index cf32527b4..fb8150ddf 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui @@ -62,7 +62,7 @@ p, li { white-space: pre-wrap; } - Standard ESC 50Hz + Analog Servo 50Hz QToolButton { border: none } @@ -106,7 +106,7 @@ p, li { white-space: pre-wrap; } - Turbo PWM ESC 400Hz + Digital Servo 333Hz QToolButton { border: none } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui index b78313744..805659ebc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.ui @@ -27,7 +27,7 @@ p, li { white-space: pre-wrap; } <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;"><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;">Go ahead and select the type of vehicle for which you want to create a 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 current version only provides functionality for Multirotors and Fixed-wing aircraft.)</span></p></body></html> +<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 current version only provides functionality for Multirotors and Fixed-wing aircraft.</span></p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop From 00ed28c98391dbc7776aad22453b4a89b5106c83 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Mon, 18 Aug 2014 03:43:00 +1000 Subject: [PATCH 156/718] Start adding PID page --- .../pages/airframestabfixedwing.cpp | 42 ++++++++++++++++ .../setupwizard/pages/airframestabfixedwing.h | 48 +++++++++++++++++++ .../pages/airframestabfixedwing.ui | 45 +++++++++++++++++ .../src/plugins/setupwizard/setupwizard.cpp | 16 +++++++ .../src/plugins/setupwizard/setupwizard.h | 4 +- .../src/plugins/setupwizard/setupwizard.pro | 9 ++-- 6 files changed, 159 insertions(+), 5 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h create mode 100644 ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp new file mode 100644 index 000000000..ae78d9254 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * + * @file notyetimplementedpage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup NotYetImplementedPage + * @{ + * @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 "airframestabfixedwing.h" +#include "ui_airframestabfixedwing.h" + +AirframeStabFixedwing::AirframeStabFixedwing(SetupWizard *wizard, QWidget *parent) : + AbstractWizardPage(wizard, parent), + ui(new Ui::AirframeStabFixedwing) +{ + ui->setupUi(this); + setFinalPage(true); +} + +AirframeStabFixedwing::~AirframeStabFixedwing() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h new file mode 100644 index 000000000..641927b7d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h @@ -0,0 +1,48 @@ +/** + ****************************************************************************** + * + * @file notyetimplementedpage.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @addtogroup + * @{ + * @addtogroup NotYetImplementedPage + * @{ + * @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 AIRFRAMESTABFIXEDWING_H +#define AIRFRAMESTABFIXEDWING_H + +#include "abstractwizardpage.h" + +namespace Ui { +class AirframeStabFixedwing; +} + +class AirframeStabFixedwing : public AbstractWizardPage { + Q_OBJECT + +public: + explicit AirframeStabFixedwing(SetupWizard *wizard, QWidget *parent = 0); + ~AirframeStabFixedwing(); + +private: + Ui::AirframeStabFixedwing *ui; +}; + +#endif // AIRFRAMESTABFIXEDWING_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui new file mode 100644 index 000000000..582c76ef4 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui @@ -0,0 +1,45 @@ + + + AirframeStabFixedwing + + + + 0 + 0 + 600 + 400 + + + + WizardPage + + + + + 20 + 170 + 551 + 51 + + + + <!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:'MS Shell Dlg 2'; font-size:8pt; font-weight:400; font-style:normal;"> +<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:12pt; font-weight:600;">TODO: This page handles PIDs, we get a choice of airframes, some generics as well as specifics like FPV Raptor and Funjet etc</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:12pt; font-weight:600;">Explain these are a starting point and likely will need further tuning as no two airframes are completely identical </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;"><br /></p></body></html> + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + true + + + + + + diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index dd82b7cf5..df335777a 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -44,6 +44,7 @@ #include "pages/rebootpage.h" #include "pages/outputcalibrationpage.h" #include "pages/revocalibrationpage.h" +#include "pages/airframestabfixedwing.h" #include "extensionsystem/pluginmanager.h" #include "vehicleconfigurationhelper.h" #include "actuatorsettings.h" @@ -138,6 +139,20 @@ int SetupWizard::nextId() const // case PAGE_REVO_CALIBRATION: // return PAGE_OUTPUT_CALIBRATION; case PAGE_OUTPUT_CALIBRATION: + { + switch (getVehicleType()) { + case VEHICLE_FIXEDWING: + return PAGE_AIRFRAMESTAB_FIXEDWING; + //TODO: Pages for Multi and heli + case VEHICLE_MULTI: + case VEHICLE_HELI: + case VEHICLE_SURFACE: + case VEHICLE_UNKNOWN: + return PAGE_SAVE; + } + } + + case PAGE_AIRFRAMESTAB_FIXEDWING: return PAGE_SAVE; case PAGE_SUMMARY: @@ -338,6 +353,7 @@ void SetupWizard::createPages() setPage(PAGE_SAVE, new SavePage(this)); setPage(PAGE_REBOOT, new RebootPage(this)); setPage(PAGE_NOTYETIMPLEMENTED, new NotYetImplementedPage(this)); + setPage(PAGE_AIRFRAMESTAB_FIXEDWING, new AirframeStabFixedwing(this)); setPage(PAGE_END, new OPEndPage(this)); setStartId(PAGE_START); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h index c941561af..d914b994c 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h @@ -158,8 +158,8 @@ private: enum { PAGE_START, PAGE_CONTROLLER, PAGE_VEHICLES, PAGE_MULTI, PAGE_FIXEDWING, PAGE_HELI, PAGE_SURFACE, PAGE_INPUT, PAGE_OUTPUT, PAGE_OUTPUT_FIXEDWING, PAGE_BIAS_CALIBRATION,PAGE_REVO_CALIBRATION, PAGE_OUTPUT_CALIBRATION, - PAGE_SAVE, PAGE_SUMMARY,PAGE_NOTYETIMPLEMENTED, PAGE_REBOOT, PAGE_END, - PAGE_UPDATE }; + PAGE_SAVE, PAGE_SUMMARY, PAGE_NOTYETIMPLEMENTED, PAGE_AIRFRAMESTAB_FIXEDWING, + PAGE_REBOOT, PAGE_END, PAGE_UPDATE }; void createPages(); bool saveHardwareSettings() const; bool canAutoUpdate() const; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro index 51a98c880..9296cd5a7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -36,7 +36,8 @@ HEADERS += setupwizardplugin.h \ pages/autoupdatepage.h \ pages/revocalibrationpage.h \ biascalibrationutil.h \ - pages/biascalibrationpage.h + pages/biascalibrationpage.h \ + pages/airframestabfixedwing.h SOURCES += setupwizardplugin.cpp \ setupwizard.cpp \ @@ -64,7 +65,8 @@ SOURCES += setupwizardplugin.cpp \ pages/autoupdatepage.cpp \ pages/revocalibrationpage.cpp \ biascalibrationutil.cpp \ - pages/biascalibrationpage.cpp + pages/biascalibrationpage.cpp \ + pages/airframestabfixedwing.cpp OTHER_FILES += SetupWizard.pluginspec @@ -88,7 +90,8 @@ FORMS += \ pages/savepage.ui \ pages/autoupdatepage.ui \ pages/revocalibrationpage.ui \ - pages/biascalibrationpage.ui + pages/biascalibrationpage.ui \ + pages/airframestabfixedwing.ui RESOURCES += \ wizardResources.qrc From 137a6e4308ce41af83228972f668c46737aec525 Mon Sep 17 00:00:00 2001 From: Fredrik Larson Date: Mon, 18 Aug 2014 03:47:16 +1000 Subject: [PATCH 157/718] Refactor name of legacy servos, remove PID hard coding from the config helper --- .../setupwizard/pages/outputpagefixedwing.cpp | 2 +- .../src/plugins/setupwizard/setupwizard.cpp | 2 +- .../vehicleconfigurationhelper.cpp | 54 ++----------------- .../setupwizard/vehicleconfigurationhelper.h | 2 +- .../setupwizard/vehicleconfigurationsource.h | 2 +- 5 files changed, 9 insertions(+), 53 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp index b635a5e55..6999877a7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp @@ -47,7 +47,7 @@ bool OutputPageFixedwing::validatePage() if (ui->ServoTypeButton->isChecked()) { getWizard()->setActuatorType(SetupWizard::SERVO_DIGITAL); } else { - getWizard()->setActuatorType(SetupWizard::SERVO_LEGACY); + getWizard()->setActuatorType(SetupWizard::SERVO_ANALOG); } return true; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index df335777a..ec0903ed7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -314,7 +314,7 @@ QString SetupWizard::getSummaryText() case ESC_RAPID: summary.append(tr("Rapid ESC (400 Hz)")); break; - case SERVO_LEGACY: + case SERVO_ANALOG: summary.append(tr("Analog Servos (50 Hz)")); break; case SERVO_DIGITAL: diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index dbca5b044..489397736 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -42,7 +42,7 @@ const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; -const qint16 VehicleConfigurationHelper::LEGACY_SERVO_FREQUENCE = 50; +const qint16 VehicleConfigurationHelper::ANALOG_SERVO_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::DIGITAL_SERVO_FREQUENCE = 333; VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource) @@ -336,13 +336,13 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() } for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { - data.ChannelUpdateFreq[i] = LEGACY_SERVO_FREQUENCE; + data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; } - qint16 updateFrequence = LEGACY_SERVO_FREQUENCE; + qint16 updateFrequence = ANALOG_SERVO_FREQUENCE; switch (m_configSource->getActuatorType()) { - case VehicleConfigurationSource::SERVO_LEGACY: - updateFrequence = LEGACY_SERVO_FREQUENCE; + case VehicleConfigurationSource::SERVO_ANALOG: + updateFrequence = ANALOG_SERVO_FREQUENCE; break; case VehicleConfigurationSource::SERVO_DIGITAL: updateFrequence = DIGITAL_SERVO_FREQUENCE; @@ -354,50 +354,6 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() actSettings->setData(data); addModifiedObject(actSettings, tr("Writing actuator settings")); - // I think I need to add a saved stabilizationsettings object also. - // unfortunately not sure this is the right way to do it! - - qDebug() << "Saving Fixed Wing Default PID Data\n"; - -/* - qDebug() << "Save Fixed Wing StabilizationBank object\n"; - StabilizationBank *stabilizationBank = StabilizationBank::GetInstance(m_uavoManager); - StabilizationBank::DataFields stabBank = stabilizationBank->getData(); - Values pulled from ./build/uavobject-synthetics/gcs/stabilizationbank.h - stabBank.PitchPI[StabilizationBank::PITCHPI_ILIMIT] = 1.123; - stabilizationBank->setData(stabBank); - addModifiedObject(stabilizationBank, tr("Writing stabilization bank")); -*/ - - qDebug() << "Save Fixed Wing StabilizationSettingsBank1 object\n"; - - // Values pulled from ./build/uavobject-synthetics/gcs/stabilizationsettingsbank1.h - StabilizationSettingsBank1 *stabilizationSettingsBank = StabilizationSettingsBank1::GetInstance(m_uavoManager); - StabilizationSettingsBank1::DataFields stabSettingsBank = stabilizationSettingsBank->getData(); - stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KP] = 2.420; - stabSettingsBank.RollRatePID[StabilizationSettingsBank1::ROLLRATEPID_KI] = 2.420; - stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KP] = 2.420; - stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_KI] = 2.420; - - stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KP] = 2.420; - stabSettingsBank.PitchRatePID[StabilizationSettingsBank1::PITCHRATEPID_KI] = 2.420; - stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KP] = 2.420; - stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_KI] = 2.420; - - stabSettingsBank.YawRatePID[StabilizationSettingsBank1::YAWRATEPID_KP] = 2.420; - stabSettingsBank.YawRatePID[StabilizationSettingsBank1::YAWRATEPID_KI] = 2.420; - stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_KP] = 2.420; - stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_KI] = 2.420; - - - // need to set max servo throw via iLimit here per my notes in http://forums.openpilot.org/topic/32356-gonna-give-revo-a-run-on-my-bixler-v1-any-suggestions/#entry252556 - stabSettingsBank.PitchPI[StabilizationSettingsBank1::PITCHPI_ILIMIT] = 1.420; - stabSettingsBank.RollPI[StabilizationSettingsBank1::ROLLPI_ILIMIT] = 1.420; - stabSettingsBank.YawPI[StabilizationSettingsBank1::YAWPI_ILIMIT] = 1.420; - - stabilizationSettingsBank->setData(stabSettingsBank); - addModifiedObject(stabilizationSettingsBank, tr("Writing stabilization bank 1 settings")); - // Set up model view image here? // loop through all the window instances and check which are of the type ModelViewGadget. // per m_thread on each instance setAcFileName(QString model_file_name) and then call reloadScene() on the same object. diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h index b2e525483..91c244121 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h @@ -59,7 +59,7 @@ public: bool setupHardwareSettings(bool save = true); static const qint16 LEGACY_ESC_FREQUENCE; static const qint16 RAPID_ESC_FREQUENCE; - static const qint16 LEGACY_SERVO_FREQUENCE; + static const qint16 ANALOG_SERVO_FREQUENCE; static const qint16 DIGITAL_SERVO_FREQUENCE; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 659da60fb..375423b5b 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -61,7 +61,7 @@ public: MULTI_ROTOR_HEXA, MULTI_ROTOR_HEXA_H, MULTI_ROTOR_HEXA_X, MULTI_ROTOR_HEXA_COAX_Y, MULTI_ROTOR_OCTO, MULTI_ROTOR_OCTO_X, MULTI_ROTOR_OCTO_V, MULTI_ROTOR_OCTO_COAX_X, MULTI_ROTOR_OCTO_COAX_PLUS, FIXED_WING_AILERON, FIXED_WING_VTAIL, FIXED_WING_ELEVON, HELI_CCPM }; - enum ACTUATOR_TYPE { ESC_RAPID, ESC_LEGACY, SERVO_LEGACY, SERVO_DIGITAL, ESC_UNKNOWN }; + enum ACTUATOR_TYPE { ESC_RAPID, ESC_LEGACY, SERVO_ANALOG, SERVO_DIGITAL, ESC_UNKNOWN }; enum INPUT_TYPE { INPUT_PWM, INPUT_PPM, INPUT_SBUS, INPUT_DSMX10, INPUT_DSMX11, INPUT_DSM2, INPUT_UNKNOWN }; enum GPS_SETTING { GPS_UBX, GPS_NMEA, GPS_DISABLED }; From 53547239286128e92e17f3c6e31356098601d949 Mon Sep 17 00:00:00 2001 From: m_thread Date: Mon, 18 Aug 2014 01:20:31 +0200 Subject: [PATCH 158/718] OP-1222 Renamed some files. Fixed Wing wizard made only available on Revo Moved motor for fixed wing to channel 3 since channel 3 got its own timer. Fixed a crash in config plugin when selecting Fixed Wing. Added code in outputcalibration in Fixed Wing setup to set min and max values as well as neutral for servos. Added Controller type Nano. --- .../configfixedwingwidget.cpp | 60 +++++---- ...wing.cpp => airframestabfixedwingpage.cpp} | 14 +- ...ixedwing.h => airframestabfixedwingpage.h} | 20 +-- ...edwing.ui => airframestabfixedwingpage.ui} | 8 +- .../pages/outputcalibrationpage.cpp | 122 +++++++----------- .../setupwizard/pages/outputcalibrationpage.h | 1 + .../pages/outputcalibrationpage.ui | 44 +++---- ...efixedwing.cpp => outputfixedwingpage.cpp} | 15 +-- ...tpagefixedwing.h => outputfixedwingpage.h} | 18 +-- ...agefixedwing.ui => outputfixedwingpage.ui} | 4 +- .../plugins/setupwizard/pages/vehiclepage.cpp | 6 + .../plugins/setupwizard/pages/vehiclepage.h | 1 + .../src/plugins/setupwizard/setupwizard.cpp | 8 +- .../src/plugins/setupwizard/setupwizard.pro | 12 +- .../vehicleconfigurationhelper.cpp | 55 ++++---- .../setupwizard/vehicleconfigurationsource.h | 2 +- 16 files changed, 180 insertions(+), 210 deletions(-) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{airframestabfixedwing.cpp => airframestabfixedwingpage.cpp} (75%) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{airframestabfixedwing.h => airframestabfixedwingpage.h} (71%) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{airframestabfixedwing.ui => airframestabfixedwingpage.ui} (93%) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{outputpagefixedwing.cpp => outputfixedwingpage.cpp} (82%) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{outputpagefixedwing.h => outputfixedwingpage.h} (77%) rename ground/openpilotgcs/src/plugins/setupwizard/pages/{outputpagefixedwing.ui => outputfixedwingpage.ui} (98%) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 2fc27cd51..47b8d050a 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -43,6 +43,33 @@ const QString ConfigFixedWingWidget::CHANNELBOXNAME = QString("fixedWingChannelBox"); +ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : + VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()), + plane(NULL) +{ + m_aircraft->setupUi(this); + + plane = new QGraphicsSvgItem(); + populateChannelComboBoxes(); + + QStringList fixedWingTypes; + fixedWingTypes << "Elevator aileron rudder" << "Elevon"; + m_aircraft->fixedWingType->addItems(fixedWingTypes); + + // Set default model to "Elevator aileron rudder" + m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); + + setupUI(m_aircraft->fixedWingType->currentText()); + + connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); + updateEnableControls(); +} + +ConfigFixedWingWidget::~ConfigFixedWingWidget() +{ + delete m_aircraft; +} + QStringList ConfigFixedWingWidget::getChannelDescriptions() { // init a channel_numelem list of channel desc defaults @@ -80,31 +107,6 @@ QStringList ConfigFixedWingWidget::getChannelDescriptions() return channelDesc; } -ConfigFixedWingWidget::ConfigFixedWingWidget(QWidget *parent) : - VehicleConfig(parent), m_aircraft(new Ui_FixedWingConfigWidget()) -{ - m_aircraft->setupUi(this); - - populateChannelComboBoxes(); - - QStringList fixedWingTypes; - fixedWingTypes << "Elevator aileron rudder" << "Elevon"; - m_aircraft->fixedWingType->addItems(fixedWingTypes); - - // Set default model to "Elevator aileron rudder" - m_aircraft->fixedWingType->setCurrentIndex(m_aircraft->fixedWingType->findText("Elevator aileron rudder")); - - //setupUI(m_aircraft->fixedWingType->currentText()); - - connect(m_aircraft->fixedWingType, SIGNAL(currentIndexChanged(QString)), this, SLOT(setupUI(QString))); - updateEnableControls(); -} - -ConfigFixedWingWidget::~ConfigFixedWingWidget() -{ - delete m_aircraft; -} - void ConfigFixedWingWidget::setupUI(QString frameType) { Q_ASSERT(m_aircraft); @@ -633,15 +635,10 @@ bool ConfigFixedWingWidget::throwConfigError(QString airframeType) return error; } -/** - WHAT DOES THIS DO??? - */ void ConfigFixedWingWidget::showEvent(QShowEvent *event) { Q_UNUSED(event) - // Thit fitInView method should only be called now, once the - // widget is shown, otherwise it cannot compute its values and - // the result is usually a ahrsbargraph that is way too small. + Q_ASSERT(plane); m_aircraft->planeShape->fitInView(plane, Qt::KeepAspectRatio); } @@ -651,6 +648,7 @@ void ConfigFixedWingWidget::showEvent(QShowEvent *event) void ConfigFixedWingWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); + Q_ASSERT(plane); m_aircraft->planeShape->fitInView(plane, Qt::KeepAspectRatio); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.cpp similarity index 75% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp rename to ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.cpp index ae78d9254..c2d5eebaf 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.cpp @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file notyetimplementedpage.cpp + * @file airframestabfixedwingpage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup NotYetImplementedPage + * @addtogroup AirframeStabFixedwingPage * @{ * @brief *****************************************************************************/ @@ -25,18 +25,18 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "airframestabfixedwing.h" -#include "ui_airframestabfixedwing.h" +#include "airframestabfixedwingpage.h" +#include "ui_airframestabfixedwingpage.h" -AirframeStabFixedwing::AirframeStabFixedwing(SetupWizard *wizard, QWidget *parent) : +AirframeStabFixedwingPage::AirframeStabFixedwingPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), - ui(new Ui::AirframeStabFixedwing) + ui(new Ui::AirframeStabFixedwingPage) { ui->setupUi(this); setFinalPage(true); } -AirframeStabFixedwing::~AirframeStabFixedwing() +AirframeStabFixedwingPage::~AirframeStabFixedwingPage() { delete ui; } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.h similarity index 71% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h rename to ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.h index 641927b7d..4a6dae8dc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.h @@ -1,11 +1,11 @@ /** ****************************************************************************** * - * @file notyetimplementedpage.h + * @file airframestabfixedwingpage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ - * @addtogroup NotYetImplementedPage + * @addtogroup AirframeStabFixedwingPage * @{ * @brief *****************************************************************************/ @@ -25,24 +25,24 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef AIRFRAMESTABFIXEDWING_H -#define AIRFRAMESTABFIXEDWING_H +#ifndef AIRFRAMESTABFIXEDWINGPAGE_H +#define AIRFRAMESTABFIXEDWINGPAGE_H #include "abstractwizardpage.h" namespace Ui { -class AirframeStabFixedwing; +class AirframeStabFixedwingPage; } -class AirframeStabFixedwing : public AbstractWizardPage { +class AirframeStabFixedwingPage : public AbstractWizardPage { Q_OBJECT public: - explicit AirframeStabFixedwing(SetupWizard *wizard, QWidget *parent = 0); - ~AirframeStabFixedwing(); + explicit AirframeStabFixedwingPage(SetupWizard *wizard, QWidget *parent = 0); + ~AirframeStabFixedwingPage(); private: - Ui::AirframeStabFixedwing *ui; + Ui::AirframeStabFixedwingPage *ui; }; -#endif // AIRFRAMESTABFIXEDWING_H +#endif // AIRFRAMESTABFIXEDWINGPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.ui similarity index 93% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui rename to ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.ui index 582c76ef4..31e672b1f 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwing.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/airframestabfixedwingpage.ui @@ -1,7 +1,7 @@ - AirframeStabFixedwing - + AirframeStabFixedwingPage + 0 @@ -17,9 +17,9 @@ 20 - 170 + 130 551 - 51 + 191 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp index d724f6076..4d9c74930 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp @@ -64,6 +64,33 @@ void OutputCalibrationPage::loadSVGFile(QString file) } } +void OutputCalibrationPage::setupActuatorMinMaxAndNeutral(int motorChannelStart, int motorChannelEnd, int totalUsedChannels) +{ + // Default values for the actuator settings, extra important for + // servos since a value out of range can actually destroy the + // vehicle if unlucky. + // Motors are not that important. REMOVE propellers always!! + + for (int servoid = 0; servoid < 12; servoid++) { + if (servoid >= motorChannelStart && servoid <= motorChannelEnd) { + // Set to motor safe values + m_actuatorSettings[servoid].channelMin = 1000; + m_actuatorSettings[servoid].channelNeutral = 1000; + m_actuatorSettings[servoid].channelMax = 2000; + } else if (servoid < totalUsedChannels) { + // Set to servo safe values + m_actuatorSettings[servoid].channelMin = 1500; + m_actuatorSettings[servoid].channelNeutral = 1500; + m_actuatorSettings[servoid].channelMax = 1500; + } else { + // "Disable" these channels + m_actuatorSettings[servoid].channelMin = 1000; + m_actuatorSettings[servoid].channelNeutral = 1000; + m_actuatorSettings[servoid].channelMax = 1000; + } + } +} + void OutputCalibrationPage::setupVehicle() { m_actuatorSettings = getWizard()->getActuatorSettings(); @@ -93,13 +120,8 @@ void OutputCalibrationPage::setupVehicle() // The channel number to configure for each step. m_channelIndex << 0 << 0 << 1 << 2 << 3 << 3 << 3; - // Default values for the actuator settings, extra important for - // servos since a value out of range can actually destroy the - // vehicle if unlucky. - // Motors are not that important. REMOVE propellers always!! - m_actuatorSettings[3].channelMin = 1500; - m_actuatorSettings[3].channelNeutral = 1500; - m_actuatorSettings[3].channelMax = 1500; + setupActuatorMinMaxAndNeutral(0, 1, 3); + getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::MULTI_ROTOR_QUAD_X: @@ -108,6 +130,7 @@ void OutputCalibrationPage::setupVehicle() m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; m_channelIndex << 0 << 0 << 1 << 2 << 3; + setupActuatorMinMaxAndNeutral(0, 3, 4); break; case SetupWizard::MULTI_ROTOR_QUAD_PLUS: loadSVGFile(MULTI_SVG_FILE); @@ -115,6 +138,7 @@ void OutputCalibrationPage::setupVehicle() m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4; m_channelIndex << 0 << 0 << 1 << 2 << 3; + setupActuatorMinMaxAndNeutral(0, 3, 4); break; case SetupWizard::MULTI_ROTOR_HEXA: loadSVGFile(MULTI_SVG_FILE); @@ -122,6 +146,7 @@ void OutputCalibrationPage::setupVehicle() m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4" << "hexa-m5" << "hexa-m6"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + setupActuatorMinMaxAndNeutral(0, 5, 6); break; case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y: loadSVGFile(MULTI_SVG_FILE); @@ -129,6 +154,7 @@ void OutputCalibrationPage::setupVehicle() m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m2" << "hexa-y6-m1" << "hexa-y6-m4" << "hexa-y6-m3" << "hexa-y6-m6" << "hexa-y6-m5"; m_vehicleHighlightElementIndexes << 0 << 2 << 1 << 4 << 3 << 6 << 5; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + setupActuatorMinMaxAndNeutral(0, 5, 6); break; case SetupWizard::MULTI_ROTOR_HEXA_H: loadSVGFile(MULTI_SVG_FILE); @@ -136,92 +162,35 @@ void OutputCalibrationPage::setupVehicle() m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4" << "hexa-h-m5" << "hexa-h-m6"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + setupActuatorMinMaxAndNeutral(0, 5, 6); break; case SetupWizard::MULTI_ROTOR_HEXA_X: m_wizardIndexes << 0 << 1 << 1 << 1 << 1 << 1 << 1; m_vehicleElementIds << "hexa-x" << "hexa-x-frame" << "hexa-x-m1" << "hexa-x-m2" << "hexa-x-m3" << "hexa-x-m4" << "hexa-x-m5" << "hexa-x-m6"; m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5 << 6; m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4 << 5; + setupActuatorMinMaxAndNeutral(0, 5, 6); break; // Fixed Wing case SetupWizard::FIXED_WING_AILERON: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 2 << 2 << 2; //2 for servoCenterSlider! + m_wizardIndexes << 0 << 1 << 2 << 3 << 4 << 2 << 3 << 4 << 2 << 3 << 4 << 2 << 3 << 4; m_vehicleElementIds << "aileron" << "aileron-frame" << "aileron-motor" << "aileron-ail-left" << "aileron-ail-right" << "aileron-rudder" << "aileron-elevator"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3 << 4 << 5; - m_channelIndex << 0 << 0 << 1 << 2 << 3 << 4; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 2 << 2 << 3 << 3 << 3 << 4 << 4 << 4 << 5 << 5 << 5; + m_channelIndex << 0 << 3 << 0 << 0 << 0 << 1 << 1 << 1 << 2 << 2 << 2 << 4 << 4 << 4; - // see Servo city for an example. 1500 usec is center on MS85mg for example. - // - http://www.servocity.com/html/hs-85mg__mighty_micro.html - // make sure Aileron servo one does not go to an extreme value - // would also be nice to make these all default to 50hz so we don't shred servos. - - m_actuatorSettings[1].channelNeutral = 1500; - // make sure Aileron servo two does not go to an extreme value - m_actuatorSettings[2].channelNeutral = 1500; - // make sure Elevator servo one does not go to an extreme value - m_actuatorSettings[3].channelNeutral = 1500; - // make sure Rudder servo one does not go to an extreme value - m_actuatorSettings[4].channelNeutral = 1500; - m_actuatorSettings[4].channelNeutral = 1500; - m_actuatorSettings[5].channelNeutral = 1500; - m_actuatorSettings[6].channelNeutral = 1500; - m_actuatorSettings[7].channelNeutral = 1500; - - // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, - // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU - // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg - m_actuatorSettings[1].channelMin = 554; - m_actuatorSettings[1].channelMax = 2400; - m_actuatorSettings[2].channelMin = 554; - m_actuatorSettings[2].channelMax = 2400; - m_actuatorSettings[3].channelMin = 554; - m_actuatorSettings[3].channelMax = 2400; - m_actuatorSettings[4].channelMin = 554; - m_actuatorSettings[4].channelMax = 2400; - m_actuatorSettings[5].channelMin = 554; - m_actuatorSettings[5].channelMax = 2400; - m_actuatorSettings[6].channelMin = 554; - m_actuatorSettings[6].channelMax = 2400; - m_actuatorSettings[7].channelMin = 554; - m_actuatorSettings[7].channelMax = 2400; + setupActuatorMinMaxAndNeutral(3, 3, 5); getWizard()->setActuatorSettings(m_actuatorSettings); break; case SetupWizard::FIXED_WING_ELEVON: loadSVGFile(FIXEDWING_SVG_FILE); - m_wizardIndexes << 0 << 1 << 2 << 2; //2 for servoCenterSlider! + m_wizardIndexes << 0 << 1 << 2 << 3 << 4 << 2 << 3 << 4; m_vehicleElementIds << "elevon" << "elevon-frame" << "elevon-motor" << "elevon-left" << "elevon-right"; - m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 3; - m_channelIndex << 0 << 0 << 1 << 2; + m_vehicleHighlightElementIndexes << 0 << 1 << 2 << 2 << 2 << 3 << 3 << 3; + m_channelIndex << 0 << 3 << 0 << 0 << 0 << 1 << 1 << 1; - // make sure elevon servo one does not go to an extreme value - m_actuatorSettings[1].channelNeutral = 1500; - // make sure elevon servo two does not go to an extreme value - m_actuatorSettings[2].channelNeutral = 1500; - m_actuatorSettings[3].channelNeutral = 1500; - m_actuatorSettings[4].channelNeutral = 1500; - m_actuatorSettings[5].channelNeutral = 1500; - m_actuatorSettings[6].channelNeutral = 1500; - m_actuatorSettings[7].channelNeutral = 1500; - - // Arduino library defaults to 554 http://arduino.cc/en/Reference/ServoAttach, - // 600 is for HS85mg - http://www.servocity.com/html/hs-85mg__mighty_micro.html#.U4JEWhapKBU - // Same rules as above from the Arduino *generic* library and the servo city info for the 85mg - m_actuatorSettings[1].channelMin = 554; - m_actuatorSettings[1].channelMax = 2400; - m_actuatorSettings[2].channelMin = 554; - m_actuatorSettings[2].channelMax = 2400; - m_actuatorSettings[3].channelMin = 554; - m_actuatorSettings[3].channelMax = 2400; - m_actuatorSettings[4].channelMin = 554; - m_actuatorSettings[4].channelMax = 2400; - m_actuatorSettings[5].channelMin = 554; - m_actuatorSettings[5].channelMax = 2400; - m_actuatorSettings[6].channelMin = 554; - m_actuatorSettings[6].channelMax = 2400; - m_actuatorSettings[7].channelMin = 554; - m_actuatorSettings[7].channelMax = 2400; + setupActuatorMinMaxAndNeutral(3, 3, 3); getWizard()->setActuatorSettings(m_actuatorSettings); break; @@ -298,7 +267,7 @@ void OutputCalibrationPage::setWizardPage() ui->calibrationStack->setCurrentIndex(currentPageIndex); int currentChannel = getCurrentChannel(); - qDebug() << "Current channel: " << currentChannel+1; + qDebug() << "Current channel: " << currentChannel + 1; if (currentChannel >= 0) { if (currentPageIndex == 1) { ui->motorNeutralSlider->setValue(m_actuatorSettings[currentChannel].channelNeutral); @@ -381,7 +350,8 @@ void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked) { ui->motorNeutralButton->setText(checked ? tr("Stop") : tr("Start")); quint16 channel = getCurrentChannel(); - onStartButtonToggle(ui->motorNeutralButton, channel, m_actuatorSettings[channel].channelNeutral, 1000, ui->motorNeutralSlider); + quint16 safeValue = m_actuatorSettings[channel].channelNeutral; + onStartButtonToggle(ui->motorNeutralButton, channel, m_actuatorSettings[channel].channelNeutral, safeValue, ui->motorNeutralSlider); } void OutputCalibrationPage::onStartButtonToggle(QAbstractButton *button, quint16 channel, quint16 value, quint16 safeValue, QSlider *slider) diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h index 0ad5f428f..d851fa639 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h @@ -53,6 +53,7 @@ public: } void loadSVGFile(QString file); + void setupActuatorMinMaxAndNeutral(int motorChannelStart, int motorChannelEnd, int totalUsedChannels); protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui index f80cab6de..e69c67666 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui @@ -34,7 +34,7 @@ - 1 + 4 @@ -72,7 +72,7 @@ p, li { white-space: pre-wrap; } - <html><head/><body><p>In this step we will set the neutral rate for the motor highlighted in the illustration to the right. <br/>Please pay attention to the details and in particular the motors position and its rotation direction. Ensure the motors are spinning in the correct direction as shown in the diagram. Swap any 2 motor wires to change the direction of a motor. </p><p>To find the neutral rate for this engine, press the Start button below and slide the slider to the right until the engine just starts to spin stable. <br/><br/>When done press button again to stop.</p></body></html> + <html><head/><body><p>In this step we will set the neutral rate for the motor highlighted in the illustration to the right. <br/>Please pay attention to the details and in particular the motors position and its rotation direction. Ensure the motors are spinning in the correct direction as shown in the diagram. Swap any 2 motor wires to change the direction of a motor. </p><p>To find <span style=" font-weight:600;">the neutral rate for this engine</span>, press the Start button below and slide the slider to the right until the engine just starts to spin stable. <br/><br/>When done press button again to stop.</p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -91,13 +91,13 @@ p, li { white-space: pre-wrap; } 1000 - 1300 + 1500 - 10 + 1 - 20 + 10 Qt::Horizontal @@ -133,7 +133,7 @@ p, li { white-space: pre-wrap; } - <html><head/><body><p>This step calibrates the center position of the servo. To set the center position for this servo, press the Start button below and slide the slider to center the servo. </p><p>When done press button again to stop.</p></body></html> + <html><head/><body><p>This step calibrates<span style=" font-weight:600;"> the center position of the servo</span>. To set the center position for this servo, press the Start button below and slide the slider to center the servo. </p><p>When done press button again to stop.</p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -149,16 +149,16 @@ p, li { white-space: pre-wrap; } false - 1000 + 600 - 2000 + 2400 - 10 + 1 - 20 + 10 1500 @@ -198,12 +198,12 @@ p, li { white-space: pre-wrap; } - + - <html><head/><body><p>To save the servo and other hardware from damage we have to set the max and min angles for the servo. </p><p>To set the minimum angle for the servo, press the Start button below and select the top slider and slide it to the left until min angle is reached.</p><p>When done press button again to stop.</p></body></html> + <html><head/><body><p>To save the servo and other hardware from damage we have to set the max and min angles for the servo. </p><p>To set <span style=" font-weight:600;">the minimum angle for the servo</span>, press the Start button below and select the top slider and slide it to the left until min angle is reached.</p><p>When done press button again to stop.</p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -219,16 +219,16 @@ p, li { white-space: pre-wrap; } false - 1000 + 600 - 1500 + 2400 - 10 + 1 - 20 + 10 1500 @@ -268,12 +268,12 @@ p, li { white-space: pre-wrap; } - + - <html><head/><body><p>To set the maximum angle for the servo, press the Start button below and select the top slider and slide it to the right until max angle is reached.</p><p>When done press button again to stop.</p></body></html> + <html><head/><body><p>To set <span style=" font-weight:600;">the maximum angle for the servo</span>, press the Start button below and select the top slider and slide it to the right until max angle is reached.</p><p>When done press button again to stop.</p></body></html> Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop @@ -289,16 +289,16 @@ p, li { white-space: pre-wrap; } false - 1500 + 600 - 2000 + 2400 - 10 + 1 - 20 + 10 1500 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.cpp similarity index 82% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp rename to ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.cpp index 6999877a7..2977e9606 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.cpp @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file outputpage.cpp + * @file outputfixedwingpage.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ @@ -25,24 +25,23 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "outputpagefixedwing.h" -#include "ui_outputpagefixedwing.h" +#include "outputfixedwingpage.h" +#include "ui_outputfixedwingpage.h" #include "setupwizard.h" -OutputPageFixedwing::OutputPageFixedwing(SetupWizard *wizard, QWidget *parent) : +OutputFixedwingPage::OutputFixedwingPage(SetupWizard *wizard, QWidget *parent) : AbstractWizardPage(wizard, parent), - - ui(new Ui::OutputPageFixedwing) + ui(new Ui::OutputFixedwingPage) { ui->setupUi(this); } -OutputPageFixedwing::~OutputPageFixedwing() +OutputFixedwingPage::~OutputFixedwingPage() { delete ui; } -bool OutputPageFixedwing::validatePage() +bool OutputFixedwingPage::validatePage() { if (ui->ServoTypeButton->isChecked()) { getWizard()->setActuatorType(SetupWizard::SERVO_DIGITAL); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.h similarity index 77% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h rename to ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.h index f3494b85b..b68dd4fbc 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.h @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file outputpage.h + * @file outputfixedwingpage.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @addtogroup * @{ @@ -25,25 +25,25 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef OUTPUTPAGEFIXEDWING_H -#define OUTPUTPAGEFIXEDWING_H +#ifndef OUTPUTFIXEDWINGPAGE_H +#define OUTPUTFIXEDWINGPAGE_H #include "abstractwizardpage.h" namespace Ui { -class OutputPageFixedwing; +class OutputFixedwingPage; } -class OutputPageFixedwing : public AbstractWizardPage { +class OutputFixedwingPage : public AbstractWizardPage { Q_OBJECT public: - explicit OutputPageFixedwing(SetupWizard *wizard, QWidget *parent = 0); - ~OutputPageFixedwing(); + explicit OutputFixedwingPage(SetupWizard *wizard, QWidget *parent = 0); + ~OutputFixedwingPage(); bool validatePage(); private: - Ui::OutputPageFixedwing *ui; + Ui::OutputFixedwingPage *ui; }; -#endif // OUTPUTPAGEFIXEDWING_H +#endif // OUTPUTFIXEDWINGPAGE_H diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.ui similarity index 98% rename from ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui rename to ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.ui index fb8150ddf..ace236fce 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/outputpagefixedwing.ui +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/outputfixedwingpage.ui @@ -1,7 +1,7 @@ - OutputPageFixedwing - + OutputFixedwingPage + 0 diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp index b7de92bc9..a8b1780a9 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.cpp @@ -55,3 +55,9 @@ bool VehiclePage::validatePage() } return true; } + +void VehiclePage::initializePage() +{ + ui->fixedwingButton->setEnabled(getWizard()->getControllerType() == SetupWizard::CONTROLLER_REVO || + getWizard()->getControllerType() == SetupWizard::CONTROLLER_NANO); +} diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h index 737f1fc25..659ac74cd 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/vehiclepage.h @@ -41,6 +41,7 @@ public: explicit VehiclePage(SetupWizard *wizard, QWidget *parent = 0); ~VehiclePage(); bool validatePage(); + void initializePage(); private: Ui::VehiclePage *ui; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp index ec0903ed7..65e12f2ff 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.cpp @@ -36,7 +36,7 @@ #include "pages/surfacepage.h" #include "pages/inputpage.h" #include "pages/outputpage.h" -#include "pages/outputpagefixedwing.h" +#include "pages/outputfixedwingpage.h" #include "pages/biascalibrationpage.h" #include "pages/summarypage.h" #include "pages/savepage.h" @@ -44,7 +44,7 @@ #include "pages/rebootpage.h" #include "pages/outputcalibrationpage.h" #include "pages/revocalibrationpage.h" -#include "pages/airframestabfixedwing.h" +#include "pages/airframestabfixedwingpage.h" #include "extensionsystem/pluginmanager.h" #include "vehicleconfigurationhelper.h" #include "actuatorsettings.h" @@ -345,7 +345,7 @@ void SetupWizard::createPages() setPage(PAGE_SURFACE, new SurfacePage(this)); setPage(PAGE_INPUT, new InputPage(this)); setPage(PAGE_OUTPUT, new OutputPage(this)); - setPage(PAGE_OUTPUT_FIXEDWING, new OutputPageFixedwing(this)); + setPage(PAGE_OUTPUT_FIXEDWING, new OutputFixedwingPage(this)); setPage(PAGE_BIAS_CALIBRATION, new BiasCalibrationPage(this)); // setPage(PAGE_REVO_CALIBRATION, new RevoCalibrationPage(this)); setPage(PAGE_OUTPUT_CALIBRATION, new OutputCalibrationPage(this)); @@ -353,7 +353,7 @@ void SetupWizard::createPages() setPage(PAGE_SAVE, new SavePage(this)); setPage(PAGE_REBOOT, new RebootPage(this)); setPage(PAGE_NOTYETIMPLEMENTED, new NotYetImplementedPage(this)); - setPage(PAGE_AIRFRAMESTAB_FIXEDWING, new AirframeStabFixedwing(this)); + setPage(PAGE_AIRFRAMESTAB_FIXEDWING, new AirframeStabFixedwingPage(this)); setPage(PAGE_END, new OPEndPage(this)); setStartId(PAGE_START); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro index 9296cd5a7..e6b32a913 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro +++ b/ground/openpilotgcs/src/plugins/setupwizard/setupwizard.pro @@ -23,7 +23,6 @@ HEADERS += setupwizardplugin.h \ pages/surfacepage.h \ pages/abstractwizardpage.h \ pages/outputpage.h \ - pages/outputpagefixedwing.h \ pages/inputpage.h \ pages/summarypage.h \ vehicleconfigurationsource.h \ @@ -37,7 +36,8 @@ HEADERS += setupwizardplugin.h \ pages/revocalibrationpage.h \ biascalibrationutil.h \ pages/biascalibrationpage.h \ - pages/airframestabfixedwing.h + pages/outputfixedwingpage.h \ + pages/airframestabfixedwingpage.h SOURCES += setupwizardplugin.cpp \ setupwizard.cpp \ @@ -52,7 +52,6 @@ SOURCES += setupwizardplugin.cpp \ pages/surfacepage.cpp \ pages/abstractwizardpage.cpp \ pages/outputpage.cpp \ - pages/outputpagefixedwing.cpp \ pages/inputpage.cpp \ pages/summarypage.cpp \ vehicleconfigurationsource.cpp \ @@ -66,7 +65,8 @@ SOURCES += setupwizardplugin.cpp \ pages/revocalibrationpage.cpp \ biascalibrationutil.cpp \ pages/biascalibrationpage.cpp \ - pages/airframestabfixedwing.cpp + pages/outputfixedwingpage.cpp \ + pages/airframestabfixedwingpage.cpp OTHER_FILES += SetupWizard.pluginspec @@ -81,7 +81,6 @@ FORMS += \ pages/helipage.ui \ pages/surfacepage.ui \ pages/outputpage.ui \ - pages/outputpagefixedwing.ui \ pages/inputpage.ui \ pages/summarypage.ui \ connectiondiagram.ui \ @@ -91,7 +90,8 @@ FORMS += \ pages/autoupdatepage.ui \ pages/revocalibrationpage.ui \ pages/biascalibrationpage.ui \ - pages/airframestabfixedwing.ui + pages/outputfixedwingpage.ui \ + pages/airframestabfixedwingpage.ui RESOURCES += \ wizardResources.qrc diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 489397736..9bd98c1df 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -231,7 +231,7 @@ void VehicleConfigurationHelper::applyVehicleConfiguration() setupElevon(); break; case VehicleConfigurationSource::FIXED_WING_VTAIL: -// setupVtail(); + // TODO: Implement settings for VTail fixed wings break; default: break; @@ -327,7 +327,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() ActuatorSettings::DataFields data = actSettings->getData(); QList actuatorSettings = m_configSource->getActuatorSettings(); - for (quint16 i = 1; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { + for (quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) { data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM; data.ChannelAddr[i] = i; data.ChannelMin[i] = actuatorSettings[i].channelMin; @@ -335,10 +335,6 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() data.ChannelMax[i] = actuatorSettings[i].channelMax; } - for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { - data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; - } - qint16 updateFrequence = ANALOG_SERVO_FREQUENCE; switch (m_configSource->getActuatorType()) { case VehicleConfigurationSource::SERVO_ANALOG: @@ -351,6 +347,19 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() break; } + for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { + if (i == 1) { + data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; + } + } else if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_NANO) { + if (i == 2) { + data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; + } + } + data.ChannelUpdateFreq[i] = updateFrequence; + } + actSettings->setData(data); addModifiedObject(actSettings, tr("Writing actuator settings")); @@ -1549,31 +1558,17 @@ void VehicleConfigurationHelper::setupElevon() channels[1].type = MIXER_TYPE_SERVO; channels[1].throttle1 = 0; channels[1].throttle2 = 0; - channels[1].roll = -127; - channels[1].pitch = 127; + channels[1].roll = -100; + channels[1].pitch = 100; channels[1].yaw = 0; channels[2].type = MIXER_TYPE_SERVO; channels[2].throttle1 = 0; channels[2].throttle2 = 0; - channels[2].roll = 127; - channels[2].pitch = -127; + channels[2].roll = 100; + channels[2].pitch = -100; channels[2].yaw = 0; - channels[3].type = MIXER_TYPE_SERVO; - channels[3].throttle1 = 0; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; - channels[3].yaw = 0; - - channels[3].type = MIXER_TYPE_SERVO; - channels[3].throttle1 = 0; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; - channels[3].yaw = 0; - guiSettings.fixedwing.FixedWingThrottle = 1; guiSettings.fixedwing.FixedWingRoll1 = 2; guiSettings.fixedwing.FixedWingRoll2 = 3; @@ -1620,12 +1615,12 @@ void VehicleConfigurationHelper::setupAileron() channels[3].pitch = 100; channels[3].yaw = 0; - channels[3].type = MIXER_TYPE_SERVO; - channels[3].throttle1 = 0; - channels[3].throttle2 = 0; - channels[3].roll = 0; - channels[3].pitch = 0; - channels[3].yaw = 100; + channels[4].type = MIXER_TYPE_SERVO; + channels[4].throttle1 = 0; + channels[4].throttle2 = 0; + channels[4].roll = 0; + channels[4].pitch = 0; + channels[4].yaw = 100; guiSettings.fixedwing.FixedWingThrottle = 1; guiSettings.fixedwing.FixedWingRoll1 = 2; diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h index 375423b5b..d03a75356 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h @@ -55,7 +55,7 @@ class VehicleConfigurationSource { public: VehicleConfigurationSource(); - enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_OPLINK }; + enum CONTROLLER_TYPE { CONTROLLER_UNKNOWN, CONTROLLER_CC, CONTROLLER_CC3D, CONTROLLER_REVO, CONTROLLER_NANO, CONTROLLER_OPLINK }; 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, From 82a3590da53f1f3afd73bf5f6219509e579aa455 Mon Sep 17 00:00:00 2001 From: m_thread Date: Mon, 18 Aug 2014 13:14:07 +0200 Subject: [PATCH 159/718] OP-1222 Fixed a bug with Actuator settings and banks to use for motors. --- .../src/plugins/coreplugin/core.qrc | 1 + .../plugins/coreplugin/images/opie_90x120.gif | Bin 0 -> 358370 bytes .../plugins/coreplugin/qml/AboutDialog.qml | 21 +++++++++++++++++- .../vehicleconfigurationhelper.cpp | 2 +- 4 files changed, 22 insertions(+), 2 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/coreplugin/images/opie_90x120.gif diff --git a/ground/openpilotgcs/src/plugins/coreplugin/core.qrc b/ground/openpilotgcs/src/plugins/coreplugin/core.qrc index 61d0155d8..f850c7aee 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/core.qrc +++ b/ground/openpilotgcs/src/plugins/coreplugin/core.qrc @@ -63,5 +63,6 @@ qml/images/tab.png qml/AboutDialog.qml ../../../../../build/openpilotgcs-synthetics/AuthorsModel.qml + images/opie_90x120.gif diff --git a/ground/openpilotgcs/src/plugins/coreplugin/images/opie_90x120.gif b/ground/openpilotgcs/src/plugins/coreplugin/images/opie_90x120.gif new file mode 100644 index 0000000000000000000000000000000000000000..417e4c032eb1bc89eedc79923c26747d144772a8 GIT binary patch literal 358370 zcmeF)S5VW7-|zchdVw@*qy_>~1Pr|j30*)8MWx9KMMVV-MJa+LbdVB26csTPrHB}$ zh>9ADfQlLr6~Ph^1nUZlE=?8)WZHa=)RWM*cz$52g6YvT~xEX{-1kYKoU&GmkYp`V}M zYP;LW$VlE1k2ouG_YqxDgVT>@m%d}BH9NEw6ck>)dL_uP9LcguOH2FfufO&?D002E za-GP%(d4$aws>c`mxVUBqqXWnRXSoU#5diS?(hHl;K+#+Cmy9+ByJ&psB*Y{Nd5P1 z-|DKWl@BG$t*$l6p0}cnZf2O7D-y0BP#ejzte|g9@gaA0b$z|)_^T)Abb$8tGa=K{ z)1T`d%XTSVxpGCkaHf*3^5lUqDo^cr^S^(E+!_a6J!g_v7c)kuI)AW zHCvps%OsDk@MoRla@*DkuGiBXD+77_pUvB5D`~rJ6>c6eKWwHpmT#Tzq0@AjGMa7G zl593t>oAsQJ&hnKEzk9RjVH&T6;Ydyo;Ey%E9QgxT-O2<#w^mKPO?NNP_ZZ=Z1rD%`N+3g#} z%O6ACY{c^~3&ZUM>6SGSn^!-T^7fHV1{!o7)>v$H&!X#2Jz)K@dBf9Oo5h);7bmP! zsp`X-mIKE&4`*83ic%lWvP^N(jtt+j-2l78h;WxlF5ay@kZd-ZW4YAgT2U0d-P7~v zAC{SJ8-M-!wfbwZn`LgMvGMf~YwX2~mDa7L#l>MecJ8;*UE(?YxJ>=l=(PNvv)JNV z>7^CAS?y`I6#xKYu~u!RX_e5Xsc=g$)_>sgIBGcMA6S~ zTH3%Luu~SXAC<}>?^;r-I>G_dfeSh3T}xxS<~9%7l2*P((!d95iz;k0Lgu=6py7sY zy>iJcL;E3;jBQhBdM(=ys%pOA8WIxTe=9-1Ry+NBckWHOxC|$jSNEM1&qXgfb0b75 zdexg(s@(*o8{r(!rtwx2ie$K`wCQpNR;iu!CjTqT9O zFU+SwY&FmCWA$b|s`<5lj32#H@jWeHM@iNTSPkO6lAHJv%Vt<#WDRea7~VM%I%_yf zgJ1D5#>hXVmeSx4Oc5>$hME!8xka_zZ)35HT6s8TN$`|ZL0jxTN;ui^)U{&JOffO9 zj1`#rrn*u)}@!1)SqGkUMJHt4PzeO&v*Nx)vtlt+fXsN^# zugUMK&VJn9ENA_Je6YLf^M#Gz#0gtyEpJo9#%fKyL~ZC~P0OpBNiH>ezV;4ovo_pQ z+0Y&A9;}U~j(@qOb;-RlB#@QoyMeePwz~bJnN@G22rikCQQsez-|SjEgOmIEU}AHJ zXQU$U@_>cpv}E>u&@S(MhQ@qIQQR_*r;4|ZuBC>Puidmblz%e)_QpiX8gk7tC{0TX z<%r+(*Evj~q9%-5ZS5Qi5BNN?;%W#w_N4NXN<%?dUELW zWpDJZSctK%pO-0@)>n0;rYPoE_C4n(KuFraVlT;isYe{*Y4M>50o6%Y^UAXR}UoEHPseR%h{0M#ErNGpFi^MWqH?YxdW;|yJybw_wO5qg5I!i zQ7zPrV9`p!ty4#iez1hSY`U$iIUW)1j9P_?VnQaAM?k`p8VSzYGIS<&(`iVh>sPY1 z*KBoV4F@yF1e$wn_c=C~_9&1N;n-|q4?2xH#NV#;HvFo1#<+0L7Rq1-)u!C1tDFw_?5#KtlpNtz)b*;`Yz|c?= zf|%^+a!`Oa(*WjsxR6Q(k&6;Bwnh!S6ANlSA+S#+|k2Bgb7}F0nlR(uDMi_IS zMTVT8&eKxtM;`;oR;%O^_$Hz(A~5H@-7%{tJ0qHtv&UQ%61vf_4AmI>u$ zeAqP~km90Y&gI~*z$YdRg&oG}H?^x~EwZGWleQ_*rzC$&HM(#I%N=-AF*7;oszYdt z9Tqeh=HfvFgPnJr@ea`ZqGSA$g^A?br|4`rYvU0WY%l_Mh=#BPJ^jTHKAjO-^lVipsF*(Y())n z<|rCFHaDP_S0Y`m!;+eDy{9%JVw<0RrrIGy*q62C**!49n)h_)P)ei;(+5?(Gw>*b z2cvt0RX#GEleQ`$Y}4|EvmQfi&*S^8>f`WCBfs=hF92+uk`K6%%hJE*q@}f3Q*1PL zZ?oaG6URIC#WbVvuWol@CoLZdfdrleS&2XUM*#Oxl1HXpaWwVtgy_)hB~S-sIdLlg|GDm1H9i}{R`(wIu4nJ_`VPz!sQ(LOhP*U zEeECBXFdLtE(y4HADz`+Z=+N#^hU5A^;_k@=6?7cqegj zZ-~W}u8i({K+3{pqX?9y&Z|xc7`(B`&kKsjSHB+4>gMSSY2HIrS$Q56s*abCP-57B zE*VpnJ9YQ-m2TBoV!CZT1%+HlN2pA*O;@^6B-KD2(BegFI13XcW7zH_>g#?>V#oeb zC=*?tMY1(?MLUxC@|~{{X>|4J5O(G1iA=lWB%E$Snry>N6^O^f$(VTQo!yf$@uNHz z*;O^3eD{?}$$beLC!KJ$5F*#}yQKH92!gk-gxaufZ5TZd*_r^}#6NV;dk{{pa`OW{ z&;QXM>aB?9LL>u5izulMipqj4bSEF_sYHtJ;N1g-R)Hj#)jpckN-<=)JD(Q8S4|y+ zLKAQUCca4>)*(F+Cj9-*C)CRrsrFKi4s19{{a6|wp+HTA$EBq8psKr3sw^7`0j^%4 zuE#>PLE-zZ+h(u{lN?O2w8TJ2a``kmkB5{Kp|ZGkZ`=shl<0e!&Mzd)=*dOU&>1@I#0X&H4Y{QC45wc(QLb7>)6d&o81M9#`bnsES zi>Ox&&@MW)M$)+67KXT5sck)J;4*O*K}vuz;bRJFF{wga1IOMPK*NPV3lk(o zLCEs}B`P*e2-Gq&8rm>)5ou}?7g8-{FbU<4Y;Js~ej0!|Qzf5BJF^%dtl)%dAH-Y; zsuf7ql91a3&;~BJmY2nJgrEe59aK=CQS|M$U8{JBUzBj114t=Bsh0yd^DsSw4naa3 z3vfx(-fqAF`~`-76g4{#)CMkP&=PE)p<9?BKPA8g-ZJnwHNII7*Kz@gm7B# zT5+`K1wo!5L8Ven9YFgQrJxk7h=`fsIdqdiQ=(mn2ABy4_Kv}zWa&=!ut~#)o;35{ zY2Y8T>R15=5WmDYbHPxQ#WVx-X)NR5SgFk@sE-N(SxPF6AtdixWU*~*1_|g`$^0A( z(=!AQowc_Rfucfm1%<3w$SOYZmrq)oFIrg$cE)r3nNq8q6G=SEN&<0qmLPKkRK$d9 z`Wj1L2SX_eEmR&;0!E%N%GKAQ4$AcAX4?bf%B0+Axk4J z_iWBd2hYYs={&GQ4ndv@^pc>!8Q3rtVnCJbqMqDY>)s~>o8U2-me_YxsKXe-g;!=G zJi3i~G zPpo{!gXnU>6I93z|MJ%qtUVPzMahsOVQbF-ulc2?d)YneinFF0WQPs2)Ji=d2ce|g zAWN)^2vYY!U0#5IGEo!v&(j5%z%wv~n^mBf7&_^~JG#Vi9nd5)cN4P;#R1;e+tpH| znKfs4g9Ne{xP7i-x|`jO$Gl>e+(N-l(=U9Um%1>9P}jjaC)G}oKvLYa{z>%|9$4jU zbrQo>r4PNCrm{&xdw`kay^}zumHnzW-I)M-$0)QG!2LPZ%lFTzpRMhauVprZVoBId zyt5-W%3iO%*9n$cBFHMq=ocMKVu0WI)q03O0DQiFlx?A)pYl->FFi-*whvAJ3RZ-`|0T)>U7g?%rSM zCiyGut6vuCvaqA>G#@%*h5WTAi7vo%xXn>+=G0&?r?0pIgD~;4BR41lk31c z$o?{`{(yTTz}!ZH!%_5c3J}b52oRzKohW_MmGq5a*S}qpo($^%pl=o_+6-=i_kKGr zqNWdB%571pg#|7ig$TfvBH9~HGNZw6{%7+FlU_MhBBR#PS=LsV-tqWa!8_{3aTcx; zxPH19vdbNIO!4}5{NYj|?IQCAgk%4w9Anzcx{3}?^(ENumHYX+e@|OicJ{qA0(TMwQ0Vy@LIOER*K@g4Hd$xK}_p`h8DF9We zk2cZXJ}Kj(N06!Q+d{s#dQDl@5>kanzvKc&6WX&*&?KPwr2|sr3^N75y%H8#j61u7 zpiwiZMX~5mp<=s);^94#8u`WjgkkArVvkrF5g4ah#4*x@9-LzGk@s8ibTnrIRL<`W z%tp=DW14x8ur9kMd{lArCL;|Re%MfxcO?&WwqV47X=8qDf1(r~Sdq~yxQ$nK4*Fd8J)z$xdl*Y2UMw+)7D@kXB0k>02p97^ z$h#eX9>Y2fcdhVX((#WTe8Ao6GI~DfnMVUnwbp+$iZ(6avr4=0TOJN)5pJee{4^zy z=VcBsdgk;Qj@Z^AO;b-O~vGV-RT-k z2Bp86TC&p`=n}Xhu3y`$^x};P;bu@`Pl+|wT1^wrKl=suE~k~%kf7szM@1@5x|X+#l~viBi-+ldJqA#6LZZr5d;KG3cs)`-t(axrVd z*>=7l63V3J$2xzQsfv)VlRH)tj@xq)p09D0R2W`3IFZs@$!}q3-S`cF-k-qre4)t? zwNJN%JSf1E*`za%`UdT{Jo|p#@Vu_T61#AI3tn{S{AF0lCbUr#PJZZhDFDU`W8v~q zD_wS7Ozf~6DzAX>mop)+esdtp;>&Lt8VBQPXj~A%H%qa!W(=}E!%pz|c)>k$rLm-i zC&Ndj1|H!iiOGInaCU01ouv^y@2q>$xWN5^(-5#^@*Vo+Tf09`TvCoQqrc&0<;WPC z?1_OtwmcZfcl#ISY!Y{Hy&kD8O%m6hLJO1+zuff+ID+b#a2E^BD{=r%4QutjOq z4%f@}9xv@Y2oddtyunxmh@j<_AI`mT&3^RSRPE5@oPzYERFKJIW9v&w`=M<&Vo7{- zx<^+t4@CmN)$~sVqs>uY(u;`2_(`dYQ|~^8S{o}OM$c6bBtKm)ogU5_kFtFyMSvWc zm+o&iK#PRGjiS5HOzQXw0a%=`z|g_<3%&*95DPjR`r>!BQ`?qOIlM#$>b;6DbkpMx zORJwp-ap>J{)$zDza>c6e?u-N>j%V%ppqIqXXt@Oo-!RGuxbBQxdrh8oE-m#Pz71w zf+ksz@kU_2c_yb@+jSfG)noR6u_jqq0m&K z|G0(yHRD(hFctsjUkM3N5#jl4+9`4y8jlfs8Z6`<#*G|{C2QC;YBnkoN$afTeU<6hFcU_Z{k zXD)eoWG{q+b~y*coe|dgza<>^B6-rm!53zXnJyAV`f~Kg-Ut4)_JxKOc3-^W5~Uqg zz#<&M5kNwSi3Z*lFecE!p?3xCyCu@Dx|nZY`|#&a_dXxqh{=!oO9@GgA6}Yb-R>yT zJ>cGS?3HU49TJB>Jj;Mo@;^lm@(-{G`eJiJ3kxE|t?n0WwO}l@++KNL08%X zqJtg+9;s>!4|gDmRlD%Ruo&)@eE@#I?bR7VHrOr$C2J5q##Dnz$n}5N%AppWI=;d5 z(Dc~wyMZG7LP2B*|3YS}&W`5r``;WhEZ{h+TL03$eR;A!I)Rr@XP@ME)#s1a^@2kGj{HFT>`i#uud9-Mg#{r{3p8Szyr1tRj&TAD#Uk= zu}Q^GC)99ju2mfYtI$Rw_%4SyJ@t%@(#hUyWYh;S+n}aZru=ofJ{_At*uzp-wyDcB zS1@GwBDT^TYp`;F*RDh2``zgUUr(|EY?>p&^UdcXXezn^%fq#Lu3~N|n{e?~G9Pg*g)s|CJOcl+!)HXxrXPLKa6o0vAxK~1O5Wj>OXZ*6-zy*wb z@?;u=xc$3qYA_XGt2i!ZpC24ZVjvIbbA)B)49-AA$w&tOEHmQNXVB4Pq%h$Wh>#hd z^4PyEJi{jk@pH^4iInA~FRY$xzNM9i_PRE+O|okY1ppbQiy1O>>$%qYO|B&fII&r1 zKdjOy-sg#sr9J&+PK4mD%>`_1o7`mW>yn^1U(A)`f@2pT{wE-y?_Dl3MDF8#Nr6Ss zex{hFIGDC=7msTTuOq}tR$F;9Pk?|5KGRf>38!2;I}uiRFj+;E{p^7A8Lobz z-7*q-PQ>ePHavliBiwU7L|48X5(Nsn@1t5;PKc86QZ*I1sM-k8`qMhaDUMapU~Zd% zQl>=TNI#ZWT+{ccX_#m^YlI@k8GAOx^U^meaS)ek?A4r~ zWy4-E%2F4cb>r&WX$my%fMmN4T&HrjY)XnZ5dM!JDc_(~N?n`p~jSfqJj-WM6U8kEbq!gh6< zN#V=;wr$>#Yp2A}OFTe>1@T5x2d|$5d|4G3H<~wCqpp<6r`TxsNH2Tk{o!q@IVy7r zmPFZcq9v?J&W~Dy4=JgAutW2B=`)F1q0eta{gBKlFY0;@ThC$ZIcz>`y9JZdr z)^pf;4qMM*>p5&ahpp$Z^&GaI!`5@ydJbF9Ve2_;J%_F5u=O0ap2OC2*m@3I&tdC1 zZ2dZH{W@&@I&A$qZ2dZH{W@&@I&A$qZ2dZH{W@&@I&A$qZ2dZH{W@&@I&A$qZ2dZH z{W@&@I&A$qZ2dZH{W@&@I&A$qZ2dZH{W@&@I&6I`aeXXteJpW(EOC7-aeXXteJpW( zEOC7-aeXXteJpW(EOC7-aeXXteJpW(EOC7-aeXXteJpW(EOC7-aeXXteJpW(EOC7- zaeXZD|A%9V=>M~J*vY@`Fh`c*|7?dTW`qEOnrxX6tCM;9i$GXLIZZp+?;dvrra`M@%zVz8A zxpE6PeqD2U0IbT9*sU`?!dh|*KvT+7wUha^(@Q_Eg}OnK&;Bc+xaZIt%Nm=1?6B{D z+hJ{#|JY$9=g*sUSeGX3#>DSkLjMTVfG;%u72gfIQ1aNAAJY52__s-<(B}A?#O~KJ zyInKJVGpBPUfVXSJu9`_7rE)xoH;70kmM^x({=lw;Outi;0w0>#*6IP|fN7x3S-@sH^9?Zakx?x8h{}8s!1( za<(E{(n}B+UH+4!9nKYUFBUEZb7T&}# zNgXg)p!!}m#d`N_k_2wcm@>ltUVW4{ zROdrulX?dl>wy*DIGa;ZnC*v>Bb8yDv!njXNvNw{_PN~Wwu}s9YD~=L*iCidK$axa zk2(8c)+%V)g5T$kD{s2%%YRW~O6!SbyL5Ta3+3~`MKPZYa_0);$K<%`J3!9QIDSz# z^B;3R-U1-=!UjX-(oNobmeQSW&3`*~MS2|k>Z2h>Yyk<4=_`{V6P@zM$mgw;J|lYF zBz5pFGb+9oD3Hs*S6x+F>j&$OGr0F%!Gw$lD|>sddCM!Goi;S7vrZq7stYo)6OJ6E zc&neh_2d!x(0uoh`$I*Ma+S!VwRK{$(YNQ>D}FwDC;iJle02Du`kIM1UeUwGCJ92r z)vhd=JIr$N)9rTSJld4p%eF5aQ7-YHevHU)@-%|B=Cc3Xpu6%Kw`o7=#=Xbc#szUD zJHs9&m}wrq?lg{w%RX6Ud!ajjM!8KQLt4!TpFL@!Wl(qOybq^fe8Q``KyKejA^PCRCZdvnhRG-i@@7)CVr~t34mL#0YKMQfR7$KBIFRC9;p)==Tx7%PMfPlA|x6g=6*{ytA^kHK-A{) zB^L&0pEU&mTXorD;xoI5IG(C!#JR6yqU(FUIDl5mCEca=vY)6qyGL78VZ}cS7$3e; zPy8b2h>y;7PYm^=vj!_ZtbkY*6#)6}+G_e`mp)~Chd7x<`Oq|`F11Q1zhm@|H9Jg4 z>ZPVuyk-B(5xIZtu$?lm?|EdK0*?XMslV+o(wZGcPSY(;^Zz-Vb$5Td>GmZm_D6m1 z{#SeVj;nWzE9}8GM&9|_g6D=z4*cDuV`AauAUn5WGc7tp`t)d3f~&yH_>7yroes!O zbo#-HzDROLV2`k3ncu#c5w>a}rFnEe((ToyQ(CVamG*Mv(B z*Yh@k4OzGyp-F6U`t>S1U1sO^jHiR4W)-=_ttwY&Z?5%Pw#Xx7gf6*r-=$pR0P($w zc?a3=)1=L(mXO~*LlDM`0KNl|;L=IlQwIUNw(=+Ri{UE7Xr*=)^zh0Np}Z;|gsP2! zFfYuTsH*J^x;NZWL4dU^j}3m* z**JscVdYN>@xKKq&(&ehP)2!85nYLp+ zas*-@3#JA{R*1lN^}`yFLY&P?fmjYKfNFdM;Pwe=GV%geXF=pO1&E}kVo|OCWNg*DYKgSd}u2RBwN2`YVrBFtGpk`k3g=E^^=%Y9?VYWZ0(MfVmS`~9+F4_ zh3yJazUFE#bwI#6+{qrhXYkW>BcB=6*X?Wp5S(MuK*`&pBrxhA7gLGZ;DmD^ekUB_ zAwuH5w8x1|_=SlrDFLcrXqvNAka$J8`5bxh%d z3ofpQt(LwUkS5<6beX_H9Yl@O)=gPYC9y26d`b@}2XP7Z4IAJ2y#mDHX4)NEBijUl zTU*{Fy(@0ENnMykz=hz$kyXPfj(kzoL@}AvoD8Z!4of&l6SgTgx5dB;Bs-&EjdpYW&pR;i>=_o0M29v}Yyw0X&3v>I z0M4Mo-XkC{_JMqW_*&6+c?O7p2emLjeRXM@j*{ZJl(($7gF;*^1-?v$l>(sO{NN>@ z-K%_*Piu->3y43Bg|IN=RL9mG&`KWrFbTBEimSyVJjEpB1y-629u#pD)czerBdPbN z!+!WimhmK3MUa{Is3uCFbvWS1!)fy!!$1l7l$@l647>>bK|U8{j)~@?yGh7tKC(w1 z;YUy1+?~5&0fY&)krIMBj3If0psgZk6C>N&7v0JxxR*)eMBw<@tTRWpjsu`L0J5nB z7vTa9U?9i2;2=J-f(cyzz6XJqP+)*61Ui}w8>|2~L_sPDaj6^>o>Z{oB)o1Q(0UkB zTLo&KO|0g)J^caQN>Pvz068M_Z35gH@Rqb!0ybU9>Os}40dI_dfH&UXz*{>Mo?LIc zZ3ednyzQ2jAQ~cGCv0sNbCD+)pbHG-QfUaBv+v7T@pT_Ng)>%Zbo8_krw4#}gRZaW zGB7TQ7!6=EbtC@6y9qh};oX$hcsG_-3ny()4)}LE_~;t%CMnFfDJxFsb)rBF>AKq*Uq3vUPo%mnuz;f?2~+W3#~#@{=C zR99$UO6oTBG&EmH154chTX>Tn{~LHy9LKEzZ(yraVE>C&{{U}9D@l`=2#uQ-*&#NY z1yZ)Ov)NB;H_$HGbATicV6tYyEt{JQahK}tZ2jFY3P1)7DDf;zYZ5oofIu5pStjoigP_9&r zW8RUV;Uuj8e$bTgGD?8)Jz#0FL9Z1=GcqA4#S-0anlv~7jh1!qm{2=>lU@ucg$kUe zU;T-Oy(ZQ9Q(8?{B_o&Hu7`l+DcIjPAW-j;o2{X@3O==TDo_wpJn+2-%`xMc31(&T z56HETI+}QBJRVA;NGY)i+tsQLL1>#eu%;#R3F?*1O>O44cu(jOr>RgV6_^wtI?vsj zq~CI+!d)0wXn~zc45)#y$xjyPV0Bo%=SGCw&A)*+{&l-=prdQR8*5AOI9%SU?#je% zX$hE6ta!)p)9s#z7Zd{i7T%aAEcS7YY0RX@63|QPw>u`2>f9xl-(8#_)mOOPS^ado z`D4SZ#6QAY)xW}<7V+k12(;wpFu@vAmG_wT;^OrY6LedD7*g6;}ZFxvBO z5+?<~%$LnIpL^NhIaV^|FdO)%WBOV_ggqe=(*1qchl|#Lw?E3d(4R7JxsUA+`YYg(&4|j4+{Y$%lm) z!l7n#oZSdskp7zo^b5WZJetC_P_R*#F^fV}cBG|^sNa`#KW7CBvp^~NqIouemI*7u zOr=#CttoPmC7AB>vitd=ZOzUyv6S4qVosvM%J~vTO|WqQTu47&&UtXE7rn?heprYm zSphq9P^SV}<>%6*+i^vBXyi_U!`RJyZkM@WjP|EU6Jk2jIfW+-^N&lHv&SPJ@x(r?OQZ*42XJRv4{lz0rg*qe`boaJRpbZUb9e5V@J87^ z;zSymdGU|%MiqcNM@K!LnqI->am(zyRj^hBk7xwR_;&Y{1vuRlx6dtKsvX5Rzb3r7 z?}pf*U+j!JE<`?E6W;vJKMM#N=$MrGL!Ypj{M@GI`R{AW`d0r6Z|+0dct>aw>+YBX z66*T5@b+rOJWKfG$Zlxjn(#(p)m8NG4q&DHCKnLK2!>LGvrkv05dq<kPLFqh9k!F#G)eIHTg3BC<1QoByTbG@ zYlDWxf{vtG4C)KaR0yjip&uwacL`n*=gIk}F0IjRzNW^W%-8RV`5IKPj&yg*We}a( z-2i=bDaG^C6A{Aa-~i z>QVZH18WYNv|51941g>OEEam6V!hz2K*sEN<*`k-Bg`a!C;ps!~9H#}dbeZWPTp%+|w5@rS`dasC<(D?14Boc(;byJ5Y0_`= zQ-PC1yN?su)>8^{lB+U%zHMFu-p-k9!5h8je8GiN+8RBw@utExjB3VZKrJ1dAU2*x zwt(h4aEi*;ccjmDge1B{Xelc9SYZ!$DLIb|2a_C;GrJ(jH`-d!*QMtjJ)@iPBVz+} za7mwbJa^V^WcuVm%W*35m_FL(V?Ts1k>AF4`1~Swm7#QNrje+3-KPid4c@Rt#q#TH zRvYs1n()RLv|bb5IPaGZ9xoxCCMvIV;rvnx*x5u$f7NH3aeviB0x++yIsL;*8lTw z+l`v3;CEc6`ET28MjlmDNRV(PyeQrr$g}{9|FPY=d!#=OZ$D&dEqq!21$Kfuf~UXO ze&s__+~iEW)SJh+#l!-9@pWpnMKoa3C9S7+tm(^AP8#6jP>2KLRt7f9>HKTvm6cTG>{rR{h6wd+PgGvF5Z zT0xzhTyvnL^h|TFIbfo+w;WIu_rBtwe&lKRpe+nvpqt;i=R)ZP z89ImBax_FQ_4TQd&haO*%Ju=vwhd~XS6$0C&EFXEg6sKhwEF9IfzN1jZwfUn1MLLP z3AnW~wMgdfD$2WKQ#se_rP1i3^2WCVe}wo+{JDY4gkEQ-Sr{CNLNp?ZPB_eLAJ|^wr+o+AG zx=V|;G(F0SPYJ+NE|*0Lox2Lns=(J(*X*#{3n~^NBrP{mbEn1LayIzk=SEkEHg{sM zLmdglQ-kr-ISBi%gVQ$1{Ze7+fJ*uOY?+`uZpO^#@AKf)1H(mj^HSEurXO4vJkQ#$ z+*flxt3L4vED=1u4HEtHOt;H>b1;;`EqQesIScM*JQV&Y0x;}b~dzo z^0D<(K5e_iJE4?Ass=yes@dyTRS>W8MDdkJ&vpj%X+q~}8ze1Rz*K&iTW46-gH^pk zuCZ6%=bG?2G(Y0GuL=dk+xn*Q_s0xB-fjtr)b~iQyN7yy z9raMLnQw-L1xa-W@kZunfg@z_kB9N6^Wxu_ApjBjbk~%8Q$maY5VCX_yZpB}Z{wcx zCdh3M`IA+!F!^Srg)tSTBN_pN;d`Ek$$t6aNjU9Ivh0^`%x68cuy}a|cf0eE>|W?? zfU{TY@Fi)M65NXMJTvr1;&r7(q1cQakt0`xh~)n8l%f5j_+gnHk{T&D5fp;vkq|wo>%t6`y z-+(}R?SfL|Xv)&no$((KvTn3;gi1o9{ZBdYnO;rj6%}Q!35a1* zHiN~6smM6JSK||#sQ1AuhEPM-N!Z?S=u*AoMJuBzn8TtAEJ@25x#FVw-cng1HXdxl z7os9%P`eiW^zG8BRV(?98=u$V&VG5wxfuo~Gk8+E9$+XQiQCl&Sfp|sOkDy{5|2B# zwzQCeR!J64MOb85F^3taKvjY*nlOv$62F0$xzmQ(fAqxoeO?^Eue9IOm7Q|jE&!AC zC`H-gNuH`Rm`+*&0ei5YKp^36`34h&4Uq-AYC%9Q)3B*?!jR3KR7z(<*{;QRFeH_V z`+2dGnRXAcr?U#CA<8r7Q&mm}{UJeO6fjvn-Z)V=+>AoD1KZ1-wsGNpjkqY&2XH|t z0Gu8HGVm)Y+w=?}>o{zmN)n#@FwZ6eOb1MIn#|%S*5`$)tW6P4Dil_?Y$x@O3Uoo$ zGKQm`bA99=3(mrOV{jPK+IMU9AKlc6e=N9~^a*IUrvFVTVpp{rk-qn-2IpbfI92ZA ztu+51v`2;IVfY1zPS0_LhzCOkX0m0EtPyfDruW4TU%Kr?pQ5; z1VnBBFoBcx#&!4h^ibjedWM&#Rk77wLkby z2rtju+jH_j@L@vKpdJ-J=SndUmLW_qJ*Mu?dCOI>!H*a}@*O2TbJ00Ph2FY1_nXYA zUYZgsT-5p4Qf*3AfQXp^*?u;6+c8d|3PlZWbW>`ti=$Up@^U&#j0eFp)bbOwk8He~ znCX9k*S!(Y@e|LbxoZG<-upWsSl5jiQ!VE<+-xXsBDkct2TA`o00&K+C&><8{^CL5 zk5(j4Ax`vHpS-ukkf;rHDi}dPg$-TSPx@4`W88CXT(Ch`k)iwXA-JyS-050j@%wRV zp&lR9E#e}#3v+CZw@DKm#=u+naWW~a0}^`tLz*wxmuaY|eRhTGW~4?|je9rPu*BUH^b0n&#a2u`%aL8l=>rS-i3O(mS=CX)xE! zq_Lw#Ir}~~*tO_J))w(#11-sgi{wMU@EnI`VnGrk;BH~IJ$?)Jvi{|GJ>B=V`LCe5 z&NIay54OL{tY+0rbhuC5c&FTvS?9Wg#oz9{Y-;kg5_a1d-!zU>GJN3h{kwh?oSX@|@oUUxN=|jKBCZQ~OL*|u*!b4# zD{g8*k1#{^jLgv#IAP$~3A4s8$}+eu4(SM$cTWylzm~|h{JkKq!6<4RDta#Tic)I?Q{5cG_WFt8t?jp<@IDA#UT;# z89WIclq*=sp-?|6+e@hqmJa^C;eX9aVmaN~}e}#GqcF#^MDZSLT3I76JCs z3XUD2+f-<54=DZQqVxDY6dl=9Vkv_|J>PrDyJ1F4Q;1llpScUGtWpY?qar?MnR6nC zR9&hw3^26Z3Bk9Fb2+_|BlheV9ad5dwC7UcnY&&h2hh$r#m!9C9r-Iza!164Urdhu zfl8)=-;lF0LHd5RX?@^Piq_957I;O2kP-~e#qP`&8y~N?uT+}OG*YEirU)5uRiHN^zC51M0B=A)-ThOC>c)ZB2T?Rz??fR{-7{19u77@sKZeW5q;d8YwlA3(!!s359Euktjd#B#1kZX1hAP@rOMeS1OXwt& z$loUNfKjHfJ`#~N;B!B|vR>%$imMd$g=-q2mZ#oH2K&vW>qipUG{6rJ=J%sE3yThL zihZFXgvzqB1SkS5mO27J)NE*Z5)s13DRijpUP9Kvpxy!ZdIx0_n#4iZlcg!OFWiD+ zKqMaTaCJh{DsOnNtD`+oX_2WT$47(>f~6ru$IVH!hyW#v)Kv?r_JMLt>LBC6ZT>we zB7d!HGH`KpN0O%+jwq=PEgN*PT^V8#xFC5B9;eiooKx*8pq$ip(Zv>L;W`ad!L096 z&WtpFJ`3&3LC7+M*}I04phsBHhVw3#sbguXsBI& zi^q;CqazEB*Vo|ARZ}ssVQkaU5U6x#R`(`Dka>n0bR*TF8{@f<%w0L_7M|gG+|sH( zlBWvhbSqrC17~s}#3dl>d2VoSGuh^?U2`v6lZ!@=O$9{9q}lD=9asJ9rI63*HaMPJ zyn8w<3Xv8PUzT4C;qQkd_`SP{ldK-`2>9sXV-vBD0arnf>h<~m!QOp7HMur?zdzGE zN$65T(|~}XDOEy8Kn#cqs2ISd*aNr}P%xo4C4dSlB{T&@3`I~x4NXuHLlFT*Ls3D| z73)Gk*k{nSuIt|OynXh}p1J>pOp-~?&++{oov86^+S}gm_~f*yz;;dygRwa48U!Km zg;2SC*<)7b`~+JSR>EfR{dpnT20ul?$;%=>u%AABY7{MjkU8*{xO*8gJ$EldCMjlo88W4pArnIV2QsSiQwPeUAX!7d(!$df?J#K{9ES0)Fc!cdqm1gA#iA`KPHBl^%VBq4G7oNeE0VjjtC z=!$HR*!?377~&BeWVbtsFbcCsX%W%M1sf`Y$BHr9ORf+yeUAd&N}UAP^tSe;Z))9& zxh*2vQVtggur*~4NH$`g4Rtq_cfXHj=el^HAxD@oXF^#0Bvd5{A4``?7o7-YBR>8u zeIeIQT}bKBf~Ya9OjyY(T=*sdHoV-SjHaxZ>0|K;SVKR4AjI!{DS-qEIA8Mzjw5C`MfXAWvMK_R*r6Zphfsm6Ui< zmsVlR;X5~R;U#tu0vjFO5C2I)pJeTAV9?*suF9h+<%uC3b-+(Hc6BQ>m+sEwCcT?* zTqH>i^UZ45=bskgn9y>-)NzN{dp|fQ-^%FZiw_V$>oRyQ&_>V**kn zoB{#0@6qH*dysMStWbgj#7H;0jSD#Q34<3Lc8G{^+k8k7)XxKvFB$PBdE{MKcuP8-fTu6vwT(sgAs!S3;wfhg1Ae^~~8-V8{V`V*a zuBw+}4FHFkn#gi<*4P0P6kac%U?Gc_rbs2175UR)el)3lZw_DnY+$s(H?fu&TDOME z=IW0Fqm-giCR~qM^ph?nFD%^kBZG0#L6wd-r4_Z2vZKXCDdCD8U!VqKGKGHTt{2I> z^xzmC3PXqKi3_8{VT-iXxw`CnAFQef>P##01ItTcUKE!mm8v*5MgAgC%U~h3mP}2w zZbIR7r5ZXAB`%HvEt5|e5kivb*a$2`l}9*v5piw~&sHjm;vx+Bz#{4LJTuXyg=`qN zX3pE3%FA#85UUyI6hk3!8s45#9oEaLn4n2HGR{S_;D_8` z>>>Cu-|}aFyU6s)l-RGlRHrB}Kr^_&1Pu>opO&XU99eLCWW~O($b-J_z|A#X zf5C_8@u1J?u%4)jQJR!J9XZZ8Cr^>ObrBd9m!6f$zGtjV(sq!oF-r0(-4JdGXIwmN zDdi%DH9yo;5Tf8Lj9BS9Lr6Gu5n(uAD>Fi1F!>V<9Ct|%^2OrfYJM$-3(Q`yEv3g^ z?la*(Bx`v=yDX|F$k{hYgcrOLAr_It<-ZSv)DEM}^rZZ6-x#Mrq?vn5jZ~xgP~+!y zQFK?8cI+A>HQlePI=L5~EGy>T%HN7PyR4Wt@AvpD{e(&{LF$Gc8hd{-o6hwZ5 zU3>UH6|IoZ$^5OD-&3z!^V~mwy_dkh z1}bLfcCz*Y`pMDXifKHuPP4(wT9jcikh5fke0{(`V_7jtd)ga|B#JqMgdW2#D<)T> zn1?~dq~GPDj@*!Lm-%FhMHdp{ph~x?C7@!GB#NmIDyHx}=O4rzU7}w3A;X4r6VV;I zc5dY6vW^@625H%ykCqiPWLYse9v0Tz`#yrM*abLbSura?#r%3|_gb}DP%%X=axL3I z#r$-k9t0vI3q141Z^hLArI?hMT-w94pkmS_im7Y~Y)-~-mDyR_W2KiBbKid{X0x?P z<+5TXIHC@_qFvDv#hg6e#p#CqVVyAfk75qspQyIq2Ng4ZSurcOkt^erFCN^jX=*CV zZf9_}Mw(^y6es;s%+EVwh`yWw{Nw8~?8|}&AYwKyX)Y_KQEy*uPa4!R zWSS_PF9a1+?`N-*pm+S2Vv3$DZX4*C>01LTCR3uA7)x?OC`!G*`JSc$B-jkU%`L69 z9z&8SrqfQ?y2?R@P0VO{AE=n#Hcx#P?t_ZyXFxV=durt{xKOJ~{YNpKnwJ%mB2i3W zSuqD+tV?7+IkFRe3{*_C>jcL^#S{t3OS^f8JYR!~$$EK1Iy<`NrQqz4S7!0C z`TiDcvl6J7l$aeAkf}o>q%j!gOTp5_iybNw#VlVCS$u_$cS4L0nSTlDmm<4bQ>&-^ z8oS;1w26B(rJN5N+(KM@wx@iY(EH3$>6I-!lcE%`+03q z^g=hTU}l*x3vRE`3Yk|n8%%a%1Wx_lVBOy#^c&FOScTA8l zg*p2`!lc|^<(Pai-^KMBF}iUIBuw7o_Jdy_V>A7-;aBO`P`iE+CNMHxFi;?B6>gCb z=4?8#ui94k!~Kcg{lhV&ndsCX0s8$8HeCJ5E8f);fOrT7SwcN(xP~aE56p4<-mS+_ z0EpL82xt7QXs!+SmJ*Z6pH>WI%Z94X)*=#0{@O%_4~t}1&hQkYC(qWwat($9_-Hi( zU0Gb?*lTDC8-K(>@*IFa}Zs;rws+RvG=tcLD4>OI1F>7gm zT`9RB_enJ<3^}29ay#E`3Tl@_;qk5!DtE-pLN)N#0UD~>cF zJ*6@DL}VB5WJ%fG#dy@3NG970w|zQmo4)9|0CHxDOQ6j{hhEKJy{gy+pX%KeS@xK- z2Adm7AADxpYGRJspB*YnT$_TvK@hE|4@^rnSpSED!5;P;;y4mZX#>0=jsTXd})`^~(K~D{{tsk!RySxaxiI0y-aSbw0oHEbe zZg=F;V$75Wf3SM3@{?~jcp3G@gv0&B!^K&1Nw&`Q{U13pP2t7yBf_-c#1&Lp)jC;y zA&@Gld_agij=UJ3rJWxn84nEc+$Iz@tacAFc%#el3^qr@sR(_~{BUm^yRCQ2 z#z@~u3uW3!*WmW>r*d~L=`+kATI|PXQ67^rwPw5XEj7;naj31e(1N=Ay7F}Coh;O* zf&(pz>5g6BD~>K$@Sa~>xn;&F zx#+`#_K0H!s+hM=&&BE*P((euG(OTs-^#X75ozuob`Q{R(&Jo@%bVgTTcdk^JX1eg(IX$Hpx-t2@wS0^5lTG|$~?1I-{Uz|4d3F@_S7`% z#3$)Tj3<{`;zruD!zY!s4xWPUIT?O$?W{#+*NPE1yy06-r);|ZR9g7pPd%W-@uFg0 zk`%qm@vgjVi#1Aa``@D#K+M)n3Etmdb5H>V)H@0EH-h9$b-l&ts0@;8pE?GfDe=n( z42fS-zJY!zP=D9|mWm1k{OB7_;TbJ1t$wDH$0e+zN+}GNyd82X2S%inMo5b(YSqGI zqvk;O&pi8Re`ege^dj=MS@;|8Mu2V#C-s<&ef4Gw5_$Hd2NHn zhgQ_CNWSO5HuBZT{Bq6?|F0Nvq;Fi}Iyu-~52qP}A)sohjN!$RPFmiO*CY5bd`j-0*2FSsOfe!z%7 zhcZt>$B4j;w9gD$@IEj1zQvNL1<;R3NTdsiVY6D`k+>E_Y;SjIbF2CqIslClRRgYH zbk}oZQ3e#wwq^+Ov+LyF{s81ayw=6U;>(IhS&U4;UN*%V^}X+Ascbu?;PXiUDcTD$ zr1WJgOW@K7giAUe`k5mIi0mYA`Qbf%S^}5azu}T~N3E;Q3GiEFvt$?$dO28kNRHn15h&>_=$5|Nzs&m(r;XBbN_{qZYpH z$BBiXPF@{~32Eccm2+(8M38MEV-XL(9rD+q+<{siK2IFDFU8L^x9%RyKAP*eb1kRt z?Es*`F3(K$Z%=ldp)0@t<1eXWLO4&%bn&6W7v}hEhDn~xX z?6b=*N}3IW=~ko4VjCi~p@XFikA|6a^Myx~8NNJX03V`C8%xnVn}(_w8ogktnVx@U zQpQ$fB2%>QDhe2aR)`tAuMp^_Z&Hs{KPAC5sVfc2*|>@4xNU{C9IRlamM*3DcDqm$ z$D$ZoQ(I-`yi)%(9MO_VEJj2Lq^#%Y{qnx-@1(H`|B5E9dmf{>Ql|gD^DS{e_I5h( z|AAf>CQJ0Pd1}pn$C&Hm1E*THV39^{ZSp7CM)utl+4WUHaittvxca)Uu7QEO@86Cv zD5b`IX-R34KN|1E_G-Bs?K$hkV6B8nhc0^av#C|UXmj-XjSk~Qb#EPUb3@U?T4lsj zIIdtgOrxML)0eFx)sdl|8hy1~KH86oPJE^4_?0R?ZhGIM#Bw>t441^1yG?(^m@4aM z*6bsudlFAAl|RmCU*F)07ws(k6boCglfRe!zgPAB?^S*OKfkKa^tF_d`b?6{i~Ck< zfir&d47P1`pMnDCBW-7@1NJ!j<`Hz-aZHd(a4IBCSi zahN%U<8zo)ZCcKQhbktZE(zlu;|07ALb`qs2NAgdmA)uu`j@`VfL6tCyKC%4H69^C zXrilejCS|wmOhQcZMR^;Q!KdQF(DR^Rx z8GBObb1JsD@)12ts<5;L%k5~9zl;&a3=~QK=yE}OjG z@aEz1z+^B~s=-0pkTv)|;P0}_K!UQ$ zi_rkUQ_howP?358@dc034VkkU^DU#@59xR1Vk#2Mxbb_=4%YtwuA2a55v`fw7^!kP$TZB6M=1Tcm_&TVsjBSr-7 zZNU0OI9xhc$wBT8lAT)rMY*_Eb%pV6ooAt=jl|HnFa~c$R{1)b2MMic3cD?}6yYGf zhZNfHX{V+jb}{P?(Sy#yi0jseQ?qzPD_>EWJU>CdoR&r#uu{*WIl0*V=%iP->nH%R zILIn2$z^Qn2ik#R+>4e}sVdwA>nu|0_5+d7>*7v`eQ4XkS*_>(d+5}IbVM|s3B-ea zFOm$6dS<0C!GW!v3r#Mbgzv%f94)*`^_zO}ZDHk9V6~r#B=oTR(a$8ooiRo)E<-+pe z{YM$0_Oe}T;>?ClNlH_t9wPqUI`z(YM@R$L;OIDfk%@eKoFKIoGrkITo8+NbgG&{y z#b`S48>C+q9Sb*xq1dQnEodoHY|C!QZMIY-H~QOFj0O`zWkVOmkkUr&BgYBtIew8O zByEBiEruZlh}(gHl>qi|mcpLuWr1JlF*A82bWva@CqzyS5` z-_n6(pygM9%5)6e3NPTLNdiCAMpIJ|2q0@4UJs0_`nIK(AWoEK%8 zZn=>l-3H!Yrdv^LD|28v=sYN11BBPN^D@G zYU==70YRRHC}X;B1oKHn_?85jFLb?rZG9>&{V5N)xqf&lvp5vgTA@U1 z+ck^tnKBobNVs{@NumR!KT_ov*B;&PIE_K)1fvy5xb$!!T#)~Dxjt3>lAAmy+k}!f z96MqkujdlAjJEtzD{h7EB=H#6<|Pev;+_7Y5IonvthW@;9!=$*kc;lU<#amT%mJeL zP7`cUuYe6If4M~UuOB{rOt6GB?^PqIOM`eZ=CFzYB=Tmx(Jnh!~3@bYxdS#S( zRl>Nut2qu*@`8%teh3KuswF;6I*?|OtHg6zSKDa$(cG%^V8yQBLX{#~C!;jE?&5K; znt#Jot0)UFOm)9y+aiRlqhmqCZr5`zTUt(38%AQmC>U8B4zsRen;v?= z11By^3RS`70(Z6kI?;YIt<~CKg7JMv;F@0s%8fF!Nt70IaQrG z7q4z>#~xv~ri3G{WZ(`FU38XS+QI5YaYsAfmUx?B>3$>sVEV%$=^i;d`3921n}xOE?};Gq9>qcpu6m8ofPm$DCS#j02DFEZ z4CU#dUfGrt4NCx=J4d53@0{F!Hi{wf-S;zg{>-A47LrI+eUgxHp?2OO^g{lr1JBToL(V*@2`3MHoOA(vsso5gCGVUOZYhSDwKIvsu#DKDW z$jy7@zspo0t&Q(Y6Wf43HVo>+#x(PL{CNb52+c{x1V&p_v5B#>$ZD>QKMnXZ39}Jz zUOL-;f5_{p0(|W@@=orZ>-D$zR?)XC>c3zY-u_}%*d++ZFg#P)QLCB z8IlyLM_rO%oPK%cvBQfhVycLk4rss0yd@nG+po(oq4@=$s2HGP@_-8?khL_C z8BndeUKHn>J2eW030@-HQ)PbkO^(Sbs*z>u$81c-7i(2}|KVNOD0R*m9z}XI?0_zb zI89*;vxCu%AI9ex@V6lEQc`!>^vxWcP=siw*G!OlW%x?3mSj*nj;d#HjdkMhj01?F zXbXRlki@_SjzV&U&uf!0O_hXdKFsuRD<@)tz9jAN$bzi88Cq4P82X(0L31*^q}~wp zT}JDPQbOxLk!pgPOEYUKIA%5bD|F2aG202`-D$dXH^{qs!9Au`5Qd=p>e(UY8PBq6 zkaypCe;}T;BWry^Z%~u#YwG&>xN)Rgcm@vTnDdmlQ+pOkGkjVGmVU-lh@R>7V-5-r zyKf>Ts{pT3Hm~_gAkRvl$xka+a~B$|!&lpUoEjM4w2-?%?|Al+6z`=s4MwV&GpS>@ zj9id5hyHqS^aUuqg^!8bDaDnZWSg<6)peE618m0II^Jm(qj$ZPuMmf}V`G!aO>wst zS)npU(wjId0T^6Iv|(V$L_>wBJ?D&^Up*-IKzOIe!HJY?Xkbj`u6w^o%FgZ`$Zy?h z@`YwFIvM)KBE$6vp*N_M;Cja;o|J4xnHgqD8(Nd&$7J)ZG+bivI`j>v!%$|M)KK@@ zb4m#DjHzOvnrZ_OZo4#fJu&YgIubq8KN9UV9dCVkEaMIJ+@<_Q?Hi{K{iGTc-T`Y+ zc=^NX1CZF8=M3MaJN-qvQUo!$E)iaMTz~(MUd*A)05N=HUf-M$s+n)ETk-nDZqXdO zkan7^w3;0F*WM#vAQ`}W3xlg&RCpBdxqyx!0dJ~Ki3)^}hY|kItpz_l++{9Z~ZA)Xf^Te=-hIrnsG}p`N^VP zGeCO#bvkYGiMnk(a??WakZ{Z)@yo)N=Z%Olr}Py@F%k7dSMEW(@2VxdOm*xjY2K7q z?~_07siq;lE5`otO5@B?CMO@%b+B{n5~L8Bbs6`3(ZMTD0=*RuxWzLcaDVi@>oDK` z{m^Ko=pEz~ylGlvr7XOsV9uW{9H$yQ*)WpWBbPdu*aBM+%&is=tN~6Heb2w;pIazX zz1_9_R%*wqGPqwo`ssK0p&t2yNu{hG$NHq2isl0!Rdg}!PsuN=9~EtC8zd&U2b9r* zkxLV;?lRiK{=o9Jq_zI|#+L78=U25RHB#66neMcQZ5M?;K2mWn;;P2_;9wFQEv$Wp?ZtGgr14(2Bjs-fy%;v-Xb> z(5V9CCCyZ&5NU;7rrXq)7RVaIiMi#Uq$mySr+i|M=;2k(mN|{buHX9aBMZ+zdVoC4 zeBtOEZTw@*H$pW^6t9#|g^VC4_@`qxx7E>M)Ov#ht(t}>(i9XUlMnNp@lgt$z)5k7 zhPpS68Y^98Q0uCE05aR5d!jbb^Ji+;hKnU(>}>-B>+6kw8F2na_>vfmSQC4>N>hSq z6=mxOe>tfcZf@fNnr9HGhG9=wQsbKY*WUT+(23rG^J9TYQjURpwkaM_4I#&ol4lDI zCxNf&!R^i0f}0*Ju=1)R`%RB@&tIrTz`U*UiexmC+kCpT)=jsWZtFhzEhZw+x3KQ# zit+G7Z$azVeBD{v^FqJQyrdr1ovdTeLywU3ao(Gr{Qb&kY?}Hsifoo?icnpW2n6gF4oyeL$!e|7rAUkR%0E32XU>YA!yLQ3>|T~>GO-l?K6 z%+&qY<5voQyw*OJc)M^jR&2D`dm0MV0+5e`w_#h+C(Bj@z%;U!&Xhay_J`6l5p!bd z>l$jD+-K+}j|pN~B*V(1%kH!kaI3e7YPL!g_gR{BTFyHeHH0o$zk5t~#dsGAQLd47 z@KgJ$06J`IkfT(o@GR#P28P`$u1y+nc zc&HLVq1K%h*nMIpIH!*h02JKI$t{46N07p~Zyb~_#70OVfv<$r_ zLdZ8V5iI)30=7@li|IeeZFtLMrihBI%093@iBT2@ zzsX1Q0l(wlM6lf}O^#U$mekFr9w^rbqOHxpvLIrV!d#}HpetDF;vWawFB-=moix=; zn0g>n51?_~4pEDOxHm7Q(Wkl^>K}y7{5Y)l)M8nGoi;7&Z!oM+qQ7L*>3{TBY6J2y+VInlFzd4$47=oNlFMSL zitmW z@v6nOFOz`U92|pVG1s!`asD5IBOK1r(T*V{Hzpp|z|ohDOt>Z0eq+s&Q75HX6R|0P zB9};o(v#|f_20skj{#SGUne9MnddrQo8^PxJfod?O6di%snSErnmx!624T*mE~*c~AG=7a>t?dJ&GLGrz8 z%}X3x>x#_^MCXLqb^F2c|1DM9)~*oz%WBJ0d4HDO>OK0N z4Rf{P4f@iHqhJQr?PD3lMitGKUtRgC!0ce5q< z%ZNsVcgwz(X&5|zds(0II1x3q`G`LQqHu9q7h$Ud$oZZ>=S{O3xnpdm!Xgt>#qlv` zTc01^+tu85h@)2jvuI}OHbS21;du1$O!^Vm{ZD>Cb(ztPk#}@ye<_fb(O35W?gH=s zqHl3ZMrq5da4BY~0+R4U*iXr`n{7EpKHJdmn%A{fIWqMed;6&;j~m@mR;{eiKFt5B zu+v$dLd!ra;xl_(rS#uI;<4hod;^b}QkZj*cCI2D-(CtXd>NjH0IV-F+6!2Y1*lk_*a)vELndb9Z zZfv@#4PWk}Z!5Uk3Dub_My9f2pD#spA_YbV)Uo2uEpy|VLe zfM+oRb;^s2rCE^winN7zd?DUuT$n(Ng-c zE&(Kl(YEF@qr>97e8%Wsr4A`i_3#K>Q9IjaZU0aT7eayhFc1dU%HxB4eh&$A8}F2r1~1a3 zb`bjctqDIQr0Y%Sc+ zmZ8l$8DFKIUaoWjl{GBEdPPa4n_RfSqO=F4^U^~U<2CQQWpJeetAbzK8`(3Q-D3ik zQo9qk6HPXi+D&c0mfUK#hXE^G3ezn80$fANi%>G8FzBpnNX%r{det&VyxPDDTc6Cd zKEpVY6kxFGrO{yKegX8tEYc4103cO;BflO-N$n9sCNZHcZ;}HFC@? z4(mor@;X^0tpT#JY%`Of9jN4RZ5+OzD?k0C1Xw+G?z)F;D$GOkY9!lP+K}?<-Mc|q z9?QYtiw1_#4i;Je@^~4nIv($+vP-?~S^MLmYPszcs={r&_Oy6|QWbrV%0UR4%5ai) zy9Dn2LY8pYyS2y#(r)y&Oh_%bhisbkdk;B5J{w5a_0hoWpwyhpW}*zXc}c5@Iva-m ze*LeBgIV!xji8Bbcl0XW|Ghtxm_m0jSnY+0RAH(bQd=~@O$@)kVRGuTPO@wxh>UfS zVyT+$Nyzq$mdaV#qz-UKnQ`6i#N?TDa7Nh>oKbd+On7A8vBw?nZzy0YovGHO74xvG zlA%)l9|3(vFY%y?piR%wVRa7~;*>EU6U>FIz(f*G-aJQ;>@1JuWEK4=@!8%*)E{Eu zV`Ss<&vhNd;pzLm!|l~e?p)vJ@(JPh+2t%Jp=0m?B$&3B=iP8)g>QKU@EKYyP<%BaW;)$8ZKl zXvN*JV8)BYFNkrbH(#Vq6hPz`iXhTd0KNBSoQkt?(i+7}kVlFMnpxacKaXc5?2134``nt{!jo-@%UJ(|8SW?X6;bfW%xSP41hy}0Ho)=RA z@TYyr>M^`&E(NZEhMBPg-}<|?%Fe%LJdxeFdQ4gYfIGp>iy4Q#818ZL$PgOj{^gT0 zV(cOdC=xjii-^Vy#yJ3gj)r*q%yR;Q8WIwd7_t@+6D%F`nu|^1BGp+yuJF{u<8fIc zVuOJAx=i~juu&!=ZtGFPS`unqEKDnwY@q@DQA>=3YiQFU=gng6eVsP*q?ClZvLeTM zFT?$nOC8l|9G`9h4F{PtIw65r0n^P)iyV^ zRU!yJxy1hiR`VSHUs!!M2lMbBtmaK2$Bykql5h*cG?ak2H%I#_8xhV8+H!Uos|Ao+ zF6I^&(?=m(U?CK1PS1;_U5!?*(NH#LK?-Te3q0b&P{v|8ktRf67aHk`kecT481={v!ck(?yrak8P|_Lb4WbMKerCqoGmF9vGayux_gJDOP(9z3A%SXP))7`zMrQ^X z27?>P%%M{gOM6hZ9b6^)##A!)a-FKSh`56bml4=+X5qk{u#X{-x_bKwP^v}QghWEWY+1JEtG$u8e{ekR8xpPB21My zv;Y&foEfLCy&{Y3MCbYCbwr*ACzF96m_@dfmiUllHJOIGz5!CdZmZT(Amru-dv!pg z3rvmKRwj&#zf$AVn0(5%f4o|locQ4ISHHXwa5kB;f02%@6m11(lOy=t1!1s)BBX}7 zJebV>J(z6sLlPN(`B6y8LEgEU_lvCCUDJwz;)3B^urn@R?u^-Oh`|uIvEl6DWM~aV z5*c^Z0gZzavetihfyub=(cwR2y%uI(Z#Z*)Q*qX6$j1ZO7@d4M3q{z&P9s~6Ggzf{mM3elm#X(f?|ae!(K2U+Zb3* zlm%$l-xT3%5+!yWvQnQaO#5xur1D?Q@o&4Pr(Xu`8VLJk*X+5gKlucPm@+;_3@ipw z87}BsQ#qmeD>!}rAjOyc=Ih#P$6KzC^6MmgEv}RBHK~^JY?-fRc4>ipO)8i0 zwV-OOb|MK;Q6Q#cfmaQ*I2A02pdL3Su z)SIpLY1gt|JD6>*{8z67uKw4)@)En&?e@(P@ohi$ohP(r%d3)s<)z+R^P-#&|MImk zTf*1;m|uKtb?KjVW#&zvCZBc@)RaURS?23Skgs`7rh*F_DHpnwGvKcO2fkkThp&gP zq7X0s!`Id?+WzHhV%44Bd@Z^M^7TB(*NmGvGG!oN*R5OT>z2k}eEp=UNy68ndyHRt zO=>g7BSv{mex%N@?mAGfU9F{V(C$dHYaTa259fjV$~9*;VAo^TeR@TSuuG8^9^o!80hF3ipA{xz@+NpG!u4h}5yHRoZA6ytdY)Q*L+ zBf+&=$cjeWPUE%f8lirqYwS7#$-0}pqtkT(e}jc_qyuo0R5a_!tcuiE5BP2tTza9i ztqSEVf?{WK2TABZAI0g^K7Nntk<`hw9{OjZaoK@N8J0?f9RcgbZ6BafdQMm zS_V}_g6l9UMR;2}aA$UVufoPcdk>hipw?z%-^l82XJ%VFlYk%;gDLZ1g;h!kMO~j< z?-I^9ZC)0I@grUNddtV3j@M(g{n}Vwm)B;^_nE+%2k%6y;I;qg6Ux;LvYh(q%Yq$< zGrvR{GI&??b;ILe8|3QK1|d7iy905q1A|Ah?=6b)qu@uHcKfWVd6r8dSSO=ygB#0z z629&mmTie|0N`@-G^(KEbYGe+4<*xq&89pns6gami$O&O0lQ5ti8A_f;O&Oc4C15U~H zB6g)Kit31v0F!oC$-wfEBg%6FI{4OkFiloUd3ojm3| z*YtF`(5goA%N`d8XIMc-{yvbel~!c`bVW(OgvRqP{L<^Siy3;6Q7Zz*YQIEWrqp0S zz0MjWfqKn(o$+NH)a%o)mh+goG{P60rQC)5a$pc_?#2SiB%}69%d*Wu7RQxKT}Xq8}(= z6O|)hR@ywfbB0WBRcuuQzc}iSpP`5HUH|@s*4^w7pf3eJ=W%}o^Y+6|gM3Yy)=R8`FZOwe zBV7O32;I1b`Dk&8wa0Vd&hbK}LKfg!t+sIJ1m%0AJ%z={uFZ=J0;Pm)ow_ZxUpvFyDfL z9kocB%{r&`f?w+1|9&0barnF8aT6zce0VO zpX>cd+)e7rxRtwY)7K?G_=4+ma0@)r3*NQY{DWpqN1pq7s@45H53ZCgVaD^9 zDhNV@EAfI1GwxSVux~62wwAR<4=C8QO`u>m6SXteJeCMHUtG*=s3wqL5#O|Yg(ACH zgRj%8CHkA(sS-51t#{lhU2esX)ip$fBbPLhj>lx$rx10vBsPnV_{JVW+%R~@=JU=g zW!DdzqQ?ou0&6S%fxjQ@(&uWc?AaaFn`BFvXIGW#efBD^)bVQn{CCxMk67zwYe*_1 zZB?`ZJp}Wv!YPwyjUxRS0gR)P5Enl>iPm)8rTA@&iI9zt9X#uAHY-!ynP9ichr&0D zI3I%Bq*@QWZjo=*jGldX-^O+Cm~s-O&S3QUK{*F(5L!tuHHc%*RilbA}-HWBf&z6le@BPVAo9g#iKG}66di$RK7;Q7x zhI^e+2fnR>q^ijwkFy6|$`W&&s{2vUVywQ<`~wNfDPMmkHhJ|aHOiWjXJ>{{SHIb( zYBfY#zHs4GN{N%$hrd$ZP)KlVmVpc2lyL=4Z0bpDnkDzD-&xs=22R-zzfH$)32SXL za7`}J(_gry&C#c3Ph;fzKkr`UFdH74r5a?4RxXx>Jkh`%Z4QBzTY98d{@7Z~Ok8oM zJM}7DTBv0eR}X1P9QWr3Ub|EvFnMB=KTxY~U(g2Ai=m`1xpb$hMO<@etDU2|Rxg24x>lsHqrq*Sud19dE9%(m$z)R3Y6V@3alH>L{C0?n6g24i|!fn z#6gEGK{vEmMTr4&8H0-Ye1ZMWI&a>#T@K^zg<6v*^~-xxPiLkkln<5boVqU)4pUnr z2rQKD^N}grADyPNS!O~nw*O|mU|mb_H;ayJ%IRile4TV=vboIA zGoIA`p%l|Y$lToe_x?1!HL8E|T;fswag_*?X)Enl68BuH;;&MTtf0$V#&rOOdb1hwf*Xsuc)36EzJDlTk;| z2km5ou0M*$ol-3NR(Q7-GM;&mT1JNuZQ5xL)deZxp22ee61Mz~kgUTpK0DO6IYrE? zuUxn#0tx%3*lP0iI6c70e3jYX$V9yiK6U8i~3KxJi(dNVtJz0_2O#;jOfR{$lOm^$EQ2`AmY z$`h*`OTsqPX!_I_GaYwxwDzZ_?QD1CD9Nrg+Byk$)tzw5GEEwDs!~3^JvcUkeg3Ut zn}%g_i4#^_Y57oWX4@5<_kGAE=y3vd8+VJwn&LQ>5P{*=QhHWxIqIjy=R|ILnzq7c zM;U)#@r9=D4R}{Vgl>A}Za2#S8P5KWF@O1~xW|1r4YuF!!Xf&~ z<|1w>teg~tq1YO>&SBTSFXgj|`#k8TSu3>Em~C!zT&Law3k`Ky`bGKV1cMjTJvjaQ z8tGjeS?#av^;1T+uO_$2Jh9tws#KI$-wFWUTv+@{Q&o?0HRn=V(q7q&!q72}hB%PY z@F9$+d<~6U_yjxRVM@Iuc08VJoOoK4T(bI_D^Pj?ovBvpvvDBF#sji4kneM4$-LB7 zx;H-MGY>}TYUjvi_NlmrT(KkwzOAo}^`>W`tVJC%AtSN$?!W@uu3qossleW+E~Ens zXSIABQBJOaM+&VS+WmS<{u{Om+gC7;=f!>=jz?8dq5eKc&p5Mym=wB`t?$^@n;%V4 zk15F-)?9CAuGW&ARq{JKOCcm6IS4T0Ond;TWds1#VCS}N-+a4QmL{)JPHHc5#L=dl6`IoEj{AtMSbKp0 zdf(=w1(}K8D&7rQB()$P@;sesnpGde<+Z#S>=I1dm1haSJIdP(zdw(cEHgIgWt^f; z)RF8pzZhH7Eg+Q>v(6uh6Hs>aasWnpmf%50oBHI`#WeX=XrskVE|I79ZRitligty* z95J&;{vY<<{4K`*@&BHEt7&Fxs%f8A?Ixv7YT8$s6cW0ogb*e|qRg~!YFcQOX^~2p zii9xjNf>)2j4XvPWw}D?d(KQ;-pl8E|8(E?aeR*E4>*6CV~#mrkJsbzybkAyBxVCC zk;EL>c`lcazpMG=>$gfgA|_q|6vEo?aYrM1b}VHo6~Nd!`+GvQBA1_hS1-ZXNvt`H zz2YE%v7f&@_X}f_ZT9-uXyZun;lD6;YHqp&V_yr{l!AjW!oLCaWj|Z@*kR-oVyE^k z|2S$BuM-YG5P0t=!4=t^0WxE!sDT9po`n#-qW?V9V$X`#)B+8o`XxPv23E_0@w^slBIYv_W`@ED=EEdsr@26%(64r}bWHKWF`?eL3j z+hl8=!xJ~wnSVTLIkO&m=yMzN%z)44aQhwTRzb^p21-%y)L}Kcc4f53#j6V(iN0$nYe0iEI_Q&IBd1T@#BlDpx*75Yf|0_ z6OWPT_(g56j!VgAN{zh&-z~lDyn-$5jCYax_OQpMEHL`8`aWnZ;j~%cG9sRhC7uO@ zkYfDZI3bl-$hLlgY6ov#6-Hm`;f^ch(=&PtyJ3VbR^58mp;Qo*ghyz;;!0Z`YK81y zUK1hLyJttmBP7vRc^Oib^8%f79a4|Ia(Fvg=Fq_Rze$?aiV;+#Pjm%p<8w#D6~*z% zt8~;a?DjCWpMXwj^l5C2ehhuNgB6hIn)n?A#vQrzGhK*8*3|RwLF5IH^M=K=ZRwyI z=2cK3F;z=rYu(r9pj-O!3&v7bFqeR)k?kc*q{djY-o|^QsZxQ=1}B($#Wk7wRE>n= zitPB$kIhgSXU?QoG9~8nJj6bPWzU<-^ALNggS_Hf`)r7>!_V-J?zFXGFTOrLulfG{ zBOia-*4+OM5WB|3epPR~!s`v_xI)4C`~V27;tlIGzf;zY$0X=$DlVoa%FX^ax4FZ( zstu$VbSEiS*lv~Bbs1)sWfw{6lw+8I<%2u^M~JOX6hZ8D6WJ!EOnZ)*Ee14Baiq#u z+Vjz#>ndganN^t6foOzvsKO2-R<&EILb|ZQXm~LW2Tt+ zd38J_shZr+ggl#ShfSKXw$KZ2%`ZiMI!I+G<@KrWyyb{a2*S)oDl4VT< zG+37e)2nwf9nV#$>oE}IPdOP|29h^D)^da#tGKmO|9R$>;Z5e7z*t85y(B}`HqZ!j zHh#Mr6uE_}?o0n_ed9|daqDgf++!k1!L~zIy(MK4J4f0SRzbQ(=P8}F1|P^wf$i|V zS9^CdPr0r=`&(hWw6Y*It&a!Jp*b&|6uRpYE93t|we}aO)}sUAUV9=jyHGE{w8KdN zc}n<;17`mxuJ#?@g}K258N3Fs+#5)R60B1U!(&{xWQsO&Ap&3{_iLMYBX{~`N`j`s z23SpfyjBaH6;1sDfCMJOS~L5v;k?|x3X?UPo?>}`c~g=#*NsQ%;O z^>%@xzd+x9sz}3TwsE<;0U-goryIBOCFF>7c(Rs`H?Bs;u`zc&Q`rgRfnd^4W$WLz zXHXJwpb#l8$V;_?+wRp5lFNMs3m9ds!y5B&dNo_1Q<1%o&$wXj4=KW@sX+?K0-_An zbiq@<;Oi@%t3;g$BgRdDRoPTSE7WS`09w3tCZyyWCwa*XEx2|ouRZ@~5@oeDOS#H5 zW=5aq75&OcE@=dmN84(DGzaDz*mA8GP^Rmq+&!6EG7Ki)kZaaruIeO3Ht9C~L`c>V zg1dh%Yqv0c*RFuVbecjvZ4c%mi~^OAYe5i9Kn`s6kS1D@dM4?wdaysU9c#El6Bw4u%;O+|KRD;P^*KgVsBbHRyNr)xj{OaFK z!OXH@#sQ@6IDDeY49VvYTIfEzi>63_}4%UU_>W-XDCTc*0@KUjm92~{_nV2d+5aCHiKlN z*E72uufMw*s(Y1^j!SitD!5#V85-u_UBr=}Ol4lwRY5K61+m{(Q>U7RsjUzEbSGwe zSC{{N$I`Wm!mjR31b^QaVmw|>3IQR=iuTx)KX5f|UF}LvyGp3Z14QozqROU_-VUVx zA%Uf?c1ULr4+Ca;55ZVWM|1i((@G}<;kQ}y_Af>a+DCTIfyEA#pZ}#nXPUnp9ddsP za+H<%@>)R_COM%3wiJ~D}`FnjCn_M znU&3@B_IxVV7F{8*YRn4%GB~NGK#zoXVP7GX%$1mKVQIr#oZ-3?XoMW*cU{PvjQ9v zk6E6Noe@S(S%7x{e+21}aCV$YzSRH7tJ#}OVA6)x%+)&R8M@RoeZ5yB=6Y02cw+pa z|AAVk9El@NxzAB+;T*NL?2Z4ES|_Xu6;W$KfP`AF1*kPYd5&5$l8gs@{-M^q_}rsO z0JU}ms5Q+e0phv}fYu>Q5wxC_Sk{y@M=`bDPff`uLKK(~eF08`oZ#Ka%(n3TFVvd* z@6@`X0L}YFt&ff&o^E!PDArAYVolxS*@y|GAe$dL?Po}-Z2;#M{6VcX{|mLAw9fwg z2er=W+%ZS3KPU1&iK#X1fN7`Lu%_+%GystugnXxF&2Yuw+HB)OF}R-47lUhBuGp~7 znUomTAxT>-x9a-Acwb|S?NvOSl?@yC5gykv+sCCmBjdb)+YEv^aQ$oG7fRS? zV<%RuT~f2W2q4E-GNO%NM&VJ2*9`VE$o>WMr~RgU~m-Fk(5!JKVPXZ~YbGfDxA6IlG!T9pAT{!-iY1Jena zbl6jFb?R^78eeceB+o0om_R`eKLjTTWtqT?G7$E5WtMNNgBS39qN?OvmH&{g$+grz zsP7>1Z$P>RRW^s@{kwF{IazQI=w)X);zO;?a8(r>+v6cksdKBpF?HBO0$eS_h9kgP zBtR|=q%{kngcMYp093_3*)oLtzj)VFjA-(AG9B~n%i@}Ebt!|eDstU2uV3EvNUg({ zZ8Ly&%}4~aYp<9&?Rv7fJOuNHc1=C;EM|^f!?$vmgGKE6_}{KIPIKtGclHugF{2b< z*93rF3lk4drY)oVV%PXUfL+@SmU}mFCG5IP%&zqScFivnv+D>6yDk&6YZnQ-=Eg1r z*mXtK9J_Wpz0dFL6PlP^v&HP1(?F20>)jzM0CvqeDPq_3rVdV3EL+U3KUIs^HMt&e zS@-$%ug*>&rdf7rFN)Ev9E6tn9O0K1lJN(sqR2aDM?-Ffkg+TZLt@K1J4Zgg4O zGQ_^TW|n;AHvh7Pgk48o{6Q;t{hM9W#q63MD`D4*f3s^)%d$Ry3A?^8X4g|9_4<+p zt!Npyu>C-m(EY!#>o339^}SQb*+0~4%Y*;0YqZZVcFh#A>n|ep`a(fv6~L}bB(H*{ZugahoF-jL#nu|iwDTPtSQ zoHOb^>Rtf4p5Jh*>%XAdfgpUkK{)ciPUQ|7eKwf;*|`j z*Td0p@j|#Jp~b<^$%n)OwXmQ#U*5McJ3Jmu9`MsKwq> z+IyvsfTpplOt3mL zWH60;p4o(s*8Ir@b}a+2YkJq=MIv^M0N6G6VL>fCr?_0guCKho{wTV1)HBE7Choy2gI!A= zFYMh_^4|K=dt+cf`oyf5UAG@DtX&4bb*ywbH7Aczbb|d9$}E-L5-VZX*8>Pn0K5Lg zO}1OK7+}|+U+kLuP{OV!Q_(NP?D`YHuJMsQH)9I<{Y>Ya+nL4Vi<2C=u>iYXBWBk> z)|+1vv+Fx9NR}rm$UwxdKNk+S-xRUyjW)m7HRZYK(~^3TdM#$xoD6wz{e^}n7gsyG zuYf5moR$8vyjG)mur<%(Zv1=_cyViV_ohM?~=f-ik zV(MrWxbZkI=&9bYD?7XwnjOzdM#~YEK;vK5-<<+(@H^# zj{)_1Y%!?0)_&}B%kl81fO`Ep$vY?S=MbP?7n%~(&b?V`{alWETZ_1f~oTR^?8dJU-8InS34MQnd3Rh;5OBK7+IvtohZ z%p={ifOv-9?k_u^s%8uxA6OXZqoNxcdjc;2cROa!n(Ja-L^N z>JhPFg2(%wU2%_-aee-LnhP(hn^YdhVXrMRy!_>XzkTEI!m{G2FTsTw*Q)C&*`)F% zr2Yf{sMo^f_sunrogD|_+6OWV6VYFmW*>JYXP*(9*AX89^Lj9#tZ$Y153b3pmks+{ z&JLV7d|Y`4@O>HH=7pLJggVV0$1Zr@C+A4=z0)SQYaEDczrC-&kM@HaKXv1v4G`Cc zEyU0>hM@eGZFRY^hu=+u@IBMtI)tL`G8(&Ra&@Q%CXS#xnc*K9>Vna=U-!~_BK}$k zy7`NC?RGz8uEEN!wPv0rUWGc2t~D9XWfa zh5D$Z8kNee!UZnbQb)>asHDr2rvPEuHoW%w2IEB6@50DsQh8aotf0D^q`dakzz?le z|L|&H-%5QkagWG-0g+TsYHH77c(yZd}>|!C-avQc6}nrH~(13r0q%b+_6;X z@M>>L?hiT(UMZ<)RrG~RvWXa z5bRM??L`JJt?>6$!AA&B!&N%HJTn%&EN!}_T|1tF?-f$gHDE8E-{l}_r)GraV2(^{s>yruR-rB@yIM;PvPGm9C!Uhe*ZYPFuVP?==9$ z-rTERu(d@guvw_2pJLh)rlpYLoArWjrUubUr8#MWQMukv@tdY>U(7y7$42ODjpRoQ zldmbbOsu>nRM^~Gmelqy zjw=(-&4#oem-+w-&8R8=E|8WoI~dTRp>TAK?e>VNoxaTxg6jLr&OKO=r*UhC>K!@q zzKX;P+L;|bgHf3QQ2ogn3Qy~(T~5J@{h)x8Zivv*8-RVysv#nP8R1KhHomV6viti} z@N7g4DY&`HfP!3|@)fz}{^#>l@q%#7!&N^WanHOLS0QZ6mh>Q&dwj`55^6)pV2`!N z3E6je#9toct(KBzTzv$b0T60DJ_PnEhnZvsnd^i)M3>vfqWBulz0Vz1Fq1Ug zdXp4*C12B1P1LEiuFtksIF^yRu7vkOoa*lb0&3a}G2DB-MKAgwq&o|>Mdr%dKr5Ne z-Jl798QrX#8pZ2QAd*)yA~d(gQF|5-60)xbNv%D@Go)&{AZ_@(wG|#UusTr(|@m^y=HQ2dq5Iu0@8rO3sG6^LByaA5Q*;C`_dXw7>oRhj)^m zvK{tYZ(cq(A)K>y3z(S-ObD|d-dSYz9A;E6Sb+;}{N3t}Xw z_`*mD@>Xk@4lZ2QB6c1jbHMOByDODQB!{>2wnS-?ZM(izu6%Sl95@?ITM(hBXSXJJ zXh15xk*+1z-myY@Z0#M9fc@A@2DY_b4q|DlK11$#0JOmrIPZDPgUnB*Jz2=9O6#Bd z6D*}!$UBy%pc#RagVUhK{(j9amAyyeli4z_%Wi8V32#gM>&mnlp0h(s&r#}>bFanh z>lGp$&LnVAf$`qfeo$yM2CK?RE^P$}h3D+_Ed3ft>V07yU zmcmXE9=#7i1!}8)+P+96U{jkE@fG-|HbV*aud2!FTQ^NYvrg!mN>8=toypd><*$dO zvH$^_die7RK)|jzt;X0QdlsZ55wP1=9pHsk0RpzevCm=wyHsC}V4kQ(>ljU>pF1WN zunB9lQ&R6LCbtz{IUv{s2-x|6fbA9ofBvSk#{lrJY1(jqCsV+`cG?)*6cN<~SJ}({ zxJM%;MTT?7A&TSF{hAW$m6BwhD!5L=RzMG>ZZL@=N6!MIa*SDHadW$4&t2it`w_7`4Sei2ry%1!yY~#1{=U5+=rz&yk4ERRld%Jdc z3*c?<+o>)5!0qE86}hg2-qEFL-4X4D5w$EY&;$YQnoUR>U|H73eNh|dpcg)GyHZ0- z{rn{aY*k~sxiu$!FY<4=js04eg+3_@T021r)Vt~vrMdKVbW>co-eY8Ld>AQSCy0H) z{#LomHGhj16|U6g!+u+vnY?Wr>S1^sKk@L_1Y`xTvmNU$E2DFd{1dpF5yOK)HNv_~ z^wRWaUW{X=ongQdAZ4ay)A?^N@HFDK6xVb2{m*gO{3~B+Kj&QkUDp-R29TX?LHE< z!z19W3NRKNUH|$#FcwTcb^Ok`augH8m5TKQ>-Cja+G@zuWNJmn$LS}XyK(K%*plWZ zraJFhTPPBCjtL(#bZII7+C)v@nSI$0Ho!Y5dvjl1GPJ~?{y)&K=NE%>yW?){7l7Op zUNRk>=NE(L7lZ$GG1$xOBz_SBl{Ns3ZSB&#WP=%ul5u}#IKDgig{@|NTh3+yBUQ7W zgzA)@?wq&@`ukuT4+fGJ$ZDA-STlFn5v_^2omNfOfjbq_Xi4BBZdz9-=t}~r>wR+{ zH4zq%E$g9z;hThs>nI%AZyx2C)rHx*Q;zWuyjGxP0`-v1PIBHPFw~8cxb8lc;0sI& z&;IA8a1$E=4fJqT@M@QOHnsADOn%EmX9#{duM0+t#wr9SfG?sF4R@HU7?DPHReeH& zcC`}^O?}wsTVH0JPwTQj!+?BeBF6Lu3p-(L0qNhV3v?n6d-=5G=vRg6k_4%1>QBQ8 zwQ|<)+EuV2|9!-r7d>DqInCqyME*s*&NZ2rCl|p(5X7l&MLk*<8dRD67cCJy0W1-R z|LYQQs>C-7M>Qvx8s#jPH!LR+ z?VN4S@mVqh*q!9TLWeAuv~=Zo*$Xy3mbIyf^#?1J62e`V`T%l97KapB=oq9=GlkP` zhc0(?RJC=y;Sk(Wb+Zs|v1;h9q1Og+4*Yw!lxqD)S5XdJpy;3qYanTd*O(hWXC2Uo zDqR;ik<%xn5SdX)N2b~>P(|2ct?FXeZL|wPW)KBmM+BD9xq733=}}XzzQ0D-zB5h3 z`pj)_@}0J~3=wv&DEH@g8T4vqA_FZIlz9o*I99U#A`r|h7PkK@D*?!Qq1H0tv=Kb~ zvMCf&RhY7IFa(U9brGo6p^W8^@Ii6JNv@Fr@kO;O*x~+>^p!N2B5cDTlHmkZ79`rm z`2{Gf(!yMu0?BxgkEk3p!W!y3s=BrL0CyQ+O_U@=6@PfhO%ZXeFy4L}=jhnC7jPr- ziZWnKkJKB4QO$|q^)Z2aQT(J2ko)*}doKtvlIfhViiPDJdyWqhZZ?Z4787y)tHh0? zIdlmT50#n;`|BtRxE<(`AipUy!2|?`*LgTOJ2NmyokF>f`esqJLhO@9w%DCE-8bh> zyDIEyb=nH_#C5yt5$BAb{ACdUxYO><4@XC`|CnSZS86}52APS8Un;lT?*p)u-^zF$Q6I#|TL)xUBfm*yyX8eP-tMfQaQLbI`PTt8d<{dw*DWq5-s11O0sIA~w(Rrm=jx6;NoF#U08DfQIjKzA zpAt2~cb!s}nw+gkIe2sGqqUZTj&_JkPXvaQeu@t}N#@oi^(T3p9lo@6WfD0$p+L$7 zG+<=6!RRFcoabzX4`w^cpS?4_>wU+L2WvbhG5b45Hmy7^V&94N8^!FqMOS|F(gf+P zOc(~#uJnPNlGFcO%3*nke2oxsa6>bAlH}_A$&6*?vT=!rU76R_4ep()FYs3S?j<*F zIMEBqcuIKkF*RYbPCs1;+z_!d1-72Ua>ZNEN0&{}bR0GIGS0#yG^GstwWMV%lXl)7 zyzy7Hs0I!p{Lo7ey3m{*f^yMQ4oQa#Suxl&_nq;uhp-%W5_jn0!t9w+mWA#A%DKGJ~> zTE|mqrmQ0faI4GZ%9_=F2f;@R&@I1cb@mvd-hGZ%p8{w#$R3{x-_74?1US}QGX+rW zpF!}xUbSVPbbVZc=4#-Ktg*r@Nex^DsDTST{0m^aneb^Q{JgHyt2`Z%V9iW6>;##1 zppt|!V*?NrfU3ENXV{+a0LR+T9XP)S9P8K7St7@pn6Q zysd>)E}3uU?^aca5tYDa>FnQvHB};5Gf9mD=}RU+*8x;botNQtnrH%YAUF3{3M!n++bmXShFTeG;8u- z|7h0q%nx1a=YSqKS*t4nKJvQ>_qu~#Kd?`w;n$XPap0Ajr z)y&v`Xtkikb;t^!)o>0u5k*Xg%xTsez86R|>wGzBqpd2&N>#^|z|KRsKP_vVZ8cz7 zZ=2P3wRe))&0+`l#<0ExY!!{ydS{n}lrgfE*tu&erd(1t9FFwJf z+9Wf;4Qy*91?dxWI_?bw&4#OV9H0G8)J4gf685bRGi*&4cfvvSOF#|R-r)8McRU4} z`%7B%3kP7T-ZnvW*czJDiu1-znt8-Nf+9hR%A=Y)gOBuyTHzWZ*;)*&@muo~Fj8zv zfwia=zElFN*)h`LhRDZ9i`~rOKl8a84!Vl}Ass{FbfHc!^r!O@oEMWox?wl@i%{p%_?? z)*;cAjj3(9>I5H=Y)y4G_ZI={Dsd`&#h<{M+pzS63mSiqK*y}Dl6Mu4Knq$$Bhch)zzB4iH#Bn)7JvQn z4_c%Ae~v)&%H~F(`K>XWYmyP@C0uAL;7RvmmvA74T&x9^ck40s1sN7bmYLiJ9uZze z^RfHAZ5+QFHRU*t^9U-tNsJPUlg=RC_lqLgntbhEBsi7|DxkM70D!gN;u>1_rs+li zST6;DHKSFQ!@YV55()roAvewwV=`#@>fLF8HtUklDA}nyie2lC+eEyYI@_wgqOU8^ zNQSL?eFPYRW^a{@KyUHsiJ|&r0j9N?x+5?Gjfc$$jbq1g1tpEEC9QBFR|Kr7J+23N z+wHE8UsP|E0Bia+wegDpu>R&H0@mbzZH3#&_d4V{0>HX`$m%hT*X+}?Ww_%{VEwlB z)^mm!STlILBgDX3cr6Y9*6d~(eXUzQG55s4`r)*;23?d2KLh}4LF4fsH}UH`X_Py} zrQL*Ow`EkCQ~_X(?@Qi|u#tnEBJrs-L-=-;#=@JO{KB-$*mLh%71iz$uvhuBr|y6N zVEtz(J=Su8`2TAxgU#* zD(i`Out9L=VdDdNKZR!go8PECDjY; zKs(w>7d@HVf!=r;J_LefF|rDIPeeP=%q(CB`s3HfBPr;fVJl2g|M53#9Z0ZRAQUdZ z!Pul{O}NiX%^StQy2}L_?TMnL9RYy#8`AJ4F|b~nmK7{%h4TdZz9lCBV9mYU49M2x z40+b67R|@aW`^b#7JP$&Ef1}r_!s1~tSFkmgo?_j#SZZ>P|nEP)ExEe0@RExW(#Hp zUuUt$Xx&fzBR_SYxM;0l6V8ji88*N26m~J?bzm@TWn7jC7iEc$dmH^~!)%=3nvbE9A9f@rA5;R2SsZ}!QMJ*s(Q^En+8ddwGbo;}G zEP}&bQnL(UDXS6ndB{N)nIv9=wgA?k0So#=lbs;%1n&vJtgW(g3dv$-Eqr_YU{>Rd zwtH12Fi@;Zznjr;HKWn=al<)a4Z09u)`IIB>acddm^J9Hs#db_X|U1N#J>QE+EUNl zmnR%m#{ji(edxn8vjDTE-+yE%c}MUB zkrh=3hG~4jwaK72a8dcfBTtS4Nq{w=r{2$+mNT+cxvyWCK|f8tmhnT((DF&sXA2M9 zt+J9{F!Lql@Z0PC$EOATD?Z|r%nV0?Vz}eyjGVSAcSQ5DzaOcHd}~FKZ(XbBKvU!76QKYD~qN0if>(jZ%ykt zbg{tu_@|F+V58AP=bsV*N8vj#211gQ)`87&(yx%-yBo5oMm_6TP{ zc-^n%g4KNTHET3Zp^7%r;gpJ8IHhuwzSbdgNMR2TSPT|9*86qR2%ReGx-0jNb8f#{ zaHv5#S2+6d*|JibJx?XTnvkR*0@nN_F|dBG<7JT>$l2$rX+E2=N`V{yN{SXp3Vw}+pb+p`Ll@%eeB@=+bN zf}LBH`&Tu+9|cm|+m$>NE1s+@*ROwV*{Z8PM2S?Nsj@>^F(~k*=9@bo=aYQ*- zL3baTHU9I*#i9b3WApO<_@k4f;CFjv-xtPaz5T$k_INzn7E2_(1sy_-x#2=RAT0ao z#=Gr=X`$Q}hupEIHsd(`&zny}c0+i_XmEne^T3aVSq|-0>*_Mcrs;4D}px!YQ4soa8jy7BAK9$wOIq?n;u!2S%PSTx|&k z!~9IU@^rsjh`PTi?nWy@8>E>VxYUrKE)23^0^t1-4H0_%1q}AsG3eL8z3DLQ6P6-rpV{98(?|jfd-SC0j zGl;(xQjjYZk;4*_q|l*1k#F+R8z$c7sgrMi-HUf>J8Sxl5T8$Pmu zZM1I9(i@U&IDNK7TfMd6EZmQAVxgSrbO`Lhp{IR_RQE>DdpLIu`cqg@k52YuYhk&SFXj~V$#}O45r%zM~eX8dU{Cb24aW( zlf)pvw|0!^gf9iJUEoDK&}PjGJKXKMm>K^;`fsC(Ui|)V`N*Z~w^mhmT>R3OQhLy% zyc*@je7x?SULAb5-K(zKh}1{A-o>-Ks1G(DN|ZkxEb*;5R;TZ4eCGZ12YhSoi=xG7 zf4ltx+Tmgdd5MB5jKbW)1B2Hu(EEZ-S|!UM-q_WZpcX|X_O}e3tbU#AdLIT(eHX84 zy>~WwL%u*JL^%k8bpnUnN#0?Zr*P)w0|+#6;htG*N0{uQC33}Xfr#C!q+$nf!E9fR z=}$cX^Uo&IHB+RV%Hglwu4s5=AiZpJnzN$wbWC$hiH#tfhPri=Qelr?9p_=yD9n+jQ zzH!0qj&}PXP*PAQK{Te&D#jnb@k{7D4F_Sj+X9B~~Y8%DF(G|JBhNGeRWzgB3hbg?NPB)ATHw^rB+{k0M z0Gv%+2|3!;A+KzfY}LP9y|?zBN;f~%=wL-?2rR@)4RMB1UNzjNBU_POgmBDe&V_w94;uLgk4&PVw_>j zSg(xI2m&F)M$iv*(LU+!mY?D)j0$^UAFGJazwOUSD=;0;Cqr5Jyv{?Z87U6En&z7RW9g6|Y4bEA*=Dib@ROZyZNV70QwlDz}p}vVvLr zOxq&e`^`{yJiz$_UP+qi!oXqT+TpOaWVHd6^B#8@!PQB>Yt&UM$hPSz+eStk+9Ew4 z+3tegnL67h#q9H2rf#ym_cCH;ECIRhPGL{!@Q|!W&yLG9IaAeDDoc27&J(uTLlEFPa^fk1{bZp9BhqyEOJSByGJy6p&>E!U~C`{TNbRevub4ca%@{>S2G`V z?>SoAj7LrkaJSi~LsAU90nOU-wYZ9`l*~(27&x818&CKs^W;6^Y3P~_2aAo#y~_G7 z8(10^ zcbq+-7v}xg$^oYzUm8#(W6kM;x97sC-rtCisj4#`=g0TtdM_Mc$`lG+2sP~q0Z|rk z@D)$HRjcy?4~`&TjOID?QJazZPaw6-KKK`KJn8%WZ&tuWw5<1j(L{9c1Emv>Wto`h zj2KU_c5yieqankX1J(^C*ABHTX##*X``Xn|Bp3kJEru?s<)0FGC2X@v09Xs`6;-$+ z>4xTWz#0RZ1sQ{Y&p}W%2!xsi&7xI3xSX~G#DUTQCbzsJ1+VQ@>M`*g$c6koajLhW zvL{qF@T1nd->McuUo^y)H-~{1_sow(htK!I z*D})slt?RgSDb%qY1>gJL`Wwl)^tZ@p7F?eg;V&@_jqBmZ-ff>RFAdvOX-yB;MFTK z={gf3*-_uQTn+q^$SP`>cH<@M9hvzFSM>HvS87*&2gfg1fOf$f{au31QqhG|D;7Pf z{~)cgyuR_ybm9_~c8@M*&VK8XmM)})`O>qzeTjE>-@Lt};M5At>fKOt!$7~q9LLV` zJ(bzJ4y1Is-3Eok_x%5*S{FdeJU(bI@f)out7K!x7@#xl_M2VK?`w5Gu!<{)bd>3>L%-TVoE(*ytm?-h*AftdHXVRju)YvcG)5@`jmv9 z`zZ%7qB6M9VSXffek6K+Bzk@%dVVB&ek6K+Bzk@%dVVB&ek6K+Bzk@%8ZedTN22FP zq5)HRek6K+Bzk@%dVVB&ek6K+Bzk@%dVVB&ek6K+Bzk@%`hV$2^gQ&P?`zMGM9)KC zRK`AZsOtXN5x%r~Jm0dhAzssBpY0(Kc>yu%i9%wE^E;)S6h27}4%%Bt=A`@8wT#RR zuYxQ)R%2?@aKnsWZTV*61t_$0WQ?-+Qazl3Aol(RZ09%_{+HQiPRG&@4CUtX!DbH- z^(1XYc2<9|Dvf)3gMf_f?LhadZ@w6p@3h^QL)F`F{d;rSgVeq9Z8qyaa@F3VTy^ts z$nPEHN^Lxi-y0UZL#zUWdVtCi&TSt0&O_gM=sOR6=b`UB^qq&k^U!x5`p!e&dFVS2 zeJ=rd>ORr5a_Ajm+kxg;5RCmD=u=-gxHUh9=2cHYzZ-W9qL=I0h=!FlUSgAq8h<#a zU+c%oy)u}zy;nZES>?U8tx0_YMMhWLrgn_Av18v2;N91#f-k40=LsB7updI%P8Fox zk~}ptfY7>G;j3IN!fw%(H$a~H?%cF8_m(72%}Pawi1XC(K%Sbt6zF2_NkK1@wyv7$ z>>luYpa`=BgkScR)H_Rma33wMOmtzxmoPn|$T^AJtWRU;3`(lI+I>6P-LLwobG9#K z@a#`VV^N;^oM>8^lPGUibs^^g(cQ^y!%^h1b@R{{YO!M;`p!e&dFVS2ednR?JoKH1 zzVpy`9{SEh-~Xl1_y6Tl<@{qFdAnSgX1(_0=?@W(kgZkOr7DRbAbw>EF~l}M-DC>1 zA+gX=|6pJj!V`K3W&d_8r=z_$Gy2naZ$HFng4|w{PPrTRkH^RUC>2(61U?9zk*~pr z_3NomS{1$@EKcrB7`>gQ@cH78M)y9@i~^a}^r~G@rQHB>R87}#ApTh8XOiAPRg&hh z{*KOR-|^I@(oTELt=5a5ZYfQLougD~%K^}r>^8p8fLZ<6VPN*Pd+1+lbr7HH{@&LH zKAwBaoEOsZI_IZFgupKD>HfAS#PuGDL+~5h8=lyo(R`5SusLFN$@a{mo>HXJxkWcE z*7-n}A(ZqVsj6=2j1r`3+y6_N8vNP9V>336yB=2k-a>Xk(;dsFp}H@-pY5JJt)-Rb zp;x40Jnd@r1nmP_?PjZOL(4^C4SBcsHoq+Nw<9I(;)FW5rIJ+g!$97Md@ida$E(|5 z)XJZBCl z=?^s5I^f0w2Go#FT{YYFVFqvN>b06NGCnf17mHI%vr+=l3gja)ZYkUUPK`cU@!kJa zr`LDXT87T!(Nu?#W*pdty!dVE7Wfi%jN|9P@#fP{+iD7bPkJ+k@BRjmL4e*1Br5_F}YafA}^-yoB={*@o=Un%I5EA>QH+3|w zP*f{^X=ZV9NkWh8$#oVEg1dVuE41=2-@d86W1_9!`-X~8tz78S)b#jyT|oPTVO}O? z747|Id}P?on##vd{UYy}TSN=YW?GYpR~dz~_m(>haUDk#2G6~3-tNBd-Iu#WcDl|w z&otIYCBx|_IHN7(i(R);%nwK9uG(-j+ERDt1=k^DR7!ri{qeTUS8C^^66G}g@hQ(O z7MPwqdd#1Fc<8ZT#bJfb`2w8tk}DAddsCu>R=UVC0tQZ0<^M`k51RsMYQrHQO?^UytLK*e@IiO0%>Y0kfwJ3ou)2w(-KR+wDrFEqyLvQ^~3+3rVcLw($vBW8{a#F zX9}ggWjC>IYr1(y7A9Gccz(9lTM`y8|3+0!*F*={`X^m1?B2eq+C-MjH+VRmEZaZT<6nnQ z(k4+=X&fE{;H{uRKX63bZrE8{IP*X+nCd~q9Djmxl)hOQ$*El=mh-U?G;Y{gsn-Lc2=UAOi^8ljs_#^+_UnzfPc(aI7nIQYAf3QvU>1cl zwRwh6q>B^wOK*b7gqiPjXJl+JBu(Z+p?O@5mZHqDcvD|TnGiccV*?+g9wC&nn92c@ z*>ALan{cZA0w{F=V$76H(I4KUgeP~|nck3*nhhsIMrld(Q#FYQVFx>P{dGIai1k)K z9&YdHP-HyzE?%tEzy{SxI+h zY_?8Rm{}uzyqYU(&P$B4i5%Q-90-*XCZOvFAk^mEaawjxV`ViOdUqW4cB0XCXebmzRF1*8JHsF#Yl|{XYqehyO5vkc+!I!53xD1IylAf$57i&}vYU3ak`IcX5>W z#luxSo`3^66&eH9JpHT8RZ7&LOK04gqw@HfI+cnVi#I|oxnQZUC#pc5$CTsfZHNl_ z%G(AnUu)zt;ijX5xOhf_`~zF@Q1a2HQcSVWZSNE^x-$BF!P;Z6C4siXe8e*ab(WQF zozB*on&hIt+bms6X%{J|#>NFZ!0tMb;VtV=UB-gJI1&#O4@$HgJG{zDnu>iryJNA! zOF!KAGBd$0A7c-PVHXRs|_(I z*go!CC`=%+?JNe=~+F3#DE zY}G`YQx4i!1Cz))>+()$F&%`0@s1QN^mi`wpb1@S4J||=gvm9C%kcdRVyTghz*vuFdZS~oUxjL|wTrSn1LVUG8m$IF1E}yxDx4xf-k`@CNfwfsLBL?} zwa$IdSHa(LP@ZhqVm|1Z08z0IYl%l%#zV>R1REB3YSH4ge6kJ|yhb-BWi#X~9hpW) z11rt9EZ}cB;A2i;-*dQZI(QqEwcGO7P&32p&k5zQherM{a6p|}Wu>@IEfm$MLnU=; zauSahcl%eJdO%XAW_mrMyJd^()G9!on(NJ)beSRVHk?o!M2hRw@dThwoi*Fyry#CV zyR`gMr^eUlJRS9@0qWG8KkC%9bwBXn89ZnSVe4ZSv{`_YVZcu^;xh3QqzKTBo8N$V*y*rv(2I{Wz`MfJAZB|-^m=Fu&^OOjTfm{Gr-3wh$0Sd zd?4$CWJ{T6W*|VY!}dtFlmqE&wF2xtwU9GH_|_UHY2l(w8jAZU%Y@H1;ozQ*;fl}M zFJKcFZIYsrWf0t~aSwQr5CP<#i>Pqi^{i1gI)_Gd)~7dbLSE>~R^}&X5HKg2)N}+m z>g;Gp%(S$fqT^!Ev3Q5fb6}6<51OkC-Q1-tCsw;s&HCGgp9@r*i38Q6HpeH9BF>D>1**>nrdLqQbCn8z2dWQszn1tQr<4crx{ti?h{iJYA9H=h(@+(ly{v%LLF7^CA7pNA^ zDL3c`^rgB9u$yfQC4uUra?%U8s+g^*s#F5I|7)OHZZ<|#sNQbvZtF(^0@cC40@a5v zRoXH0W68&XK($efikT!(4O;R$P`ziXI8a?~KEnk9)$3*$NLvMCeL*@&6a0h<9j2We z5+Y~Vn4?h448>fZmQffk<$&LrikFt5VxnwM{p4b4Ov2tz;29oF)TkC3P45Ej3jr5^ z;BHjuTTS9dHU4Z3J}lV0xX!Q;XjC677B#9>fkrjOxJX&SAjBPLRGW-Zh_os*InmTAzvCMNB zyGh-Zr&Mz&J;GBd_sXOH7*%fEDr!_;Q2J+7+3E6JqZ$}IUho|Ez7C0j0*lsp>Y8(n zYKy#o8rATs*40;lRZopn{i|i0B?hpV1#ipB=Lph&|&$LAW=lq)eH-an0M zMu+w9Mm2|b|5u}W3TRZ*t^tkeL)&azm5n8hYSrDOy_YR&ok3l{MwRCp)x36--=oUw znt(>Nvt(2m)X*UrRi-t>dtw%SvwXKiJgUq*qhY7+Iyb6(LlmiYYgJ!m*d1&nbM&CN zQO(=Bzg;Vq3)xxU6GQXKl{Bhh`l3cP-b6I2{G)M=q)|%9L zrY#;-o~;u#sx|+$QEj_tu2J3d-Rdz{JgRK>Ka48#e~l{Bc=6&rFsh9Ik5Od=(5Nn76p6<; z(jY~}qDJ+^JIiwsVBFHuF0;6Uf&T2@gCO zp6@M-V$f^o)ni-yf~eAl~=18kxz#m-S5c{tM%(qiqqPmp?rD2hgJ{nI*j0C zLGZOdtV+0#G!4tg2K(19Md-wugsvxS43VO59;lJcyUoW=0Hez91^`SO%oL3(&x)w^ z-5I*d*seh$VKHftE0X|O-LHRU3L3-r1)z0=Nl_55hFAxO@*!o+lBe|JwJJrpZ}+Ad zL1kJOnw~I)@3_1CEsB9gHMMVO7Kl{ihZBm10{Fw7-(WZ!bnY6=*iul(AiLU1y-5zE zEA*BpECw3YGTVA5*H!_G%7>tk04BPXH@B!fo{ApXZd1?1y%#Mi@9bV02P`Td`oz9% zS9}eg=*Woy7L{kHptOmlKRC}U{4+$zn(haN0WF-#QnSe=JQZ+1v8x5R6G8qhiyq=4)m{-^rSBVgsP*ro%Hxtz<&nR$fKg>> z`{V0@QDtTl5U5rb2dc>({I(HmNWx+*((=sQUNl`4s7?rm(lg#Be8~EmCJIzX1h?Qk z2mcRycm5Cc6^AD3z#QV=1(~MyZ7OjOzV9uXFi+&*gi5`FzjiaxUXPV16(&x98(}zbFQ(Bc}+# zZl@ots(Rp&PH6oNRMQ5te*)EJY6p^qYzh)AySO@)Hg-V_RMVG%Y7FvK^~gG?XDerwWTVK4G_;LM7jooNmKPb?eN*KQ5g+EozMONQ7lc6lJ)FNuehf9)#s<-AAu_>g6z`e~Q`QO3MO z{_MTI|7llwS73eu2ERCqv3>JUBlS2^-ZZS3op(!So+e#mO@44Y6Jk&}%kB@ojb2RE z9enz*rp-movni~;_Kwaz;~l4=)2ZE_WkxoWW8ZuKFwWGGw$iyKNOQoPTt#nO;g&<|AnX;eG^_-R!8 z+Iajjs*S})HKk40zkJ!Kjw*l$EF0DD8aA&`D?cFZ+h^6X-v_>7ThIaSH59jQv>4#v-t-TJF-`YLj z)^g;BdjxxLOzz85Z#RJD>K6y6lB%l=SaKJIpzNlsS7+x5B2fIoy7al_dDv7ltVswmV#8j?!BcDcRI^eZL*)Fs>%c$L1y2p7#e!}s{xoy`RlBP z{ew09Ra+)WT^q{E>p&Cq;f`*O>b1%I41X=hHQ40tY(zTA5t#xdd03Tn7LR>i%1+ap zX|6_BT5vKw{{9$Mu;85j*o)Z+5b zBET`Hp<6-dXL_3m1B+?Mpm z>UqG(mU#j4T9-s}(PuZloIJ7X`JDXCKsqqP|EQst&T~2RtmCOpR`8RmV8T2<>&a9o zbCH;cUCIL+13r!`TPUyEc6IHYCV7~Gi)3ZNE(V`=I4KaTYPrQOp0nMQZJ$CMy@5h< zLYVtOky__9R{JVcI-HHnGLx=KvNEgz8ZxWV8$Tzly)=~*vmn29C@N($EJ|~24@*j@ z<${u~&y20B(Aw4G-fEq%w_dJNN68oXy8TnE9Q`B=LE#%EUh;?=a65G^+@q>S+mgWC ztXi{!W#849{WSk|<_)37)>F8m!@-VNt)qa#r!lQZj(;I%X?LJY&eg?6Vy3!E%v1yW zN~Fa~b=gm)`pnxKlwj#)r8=03n@m!Befj<|-Dy>}SgF4A{g+Zr3a5&d>PfR@rCLfP zs{g4}FRd9iS&HdT{-snq-4iR-{^v|Wqtmm=v5%6Wypjx2ou}i;pGx&s87Yr_bGg)I zr8@eTQVqBhbno*M_wRlTxF4uC)Q4&RQmW4`E7h;QCn>3YIg4;VbqpGL$7@aEx@D!h zZCR-%Z3}GurBv66m1;o4ds(SwrmS05s-ON+s(0^r=OtFE3&cvb?k}bK?A@9rh?+1( zUy24PF7Fg8)yz48SgD>211%aGdx(|lQx==R<-e8c$qnicNB*T$*K8Fl)jP#Xbz^Ib zq8BJ+msqKmzI%B-WdFgEWA2pX%`1K?)ub0AIm=45i}!D(`i)qrzIOU%n3)y~hCfX{ zw01ipdw=92W7Cn9zm)2iCo?t_pR@Xw*kN4U(WY*+kJ zs^cquD%FCIVx>CuDE3=T?p3i;U3z8#2-}x|hCN`^eh@3wabl%gYXi zdPA3y^KYeE@I>7@pkAz0(~N#8)%tgnyQG8D24+h%mE(UY)t(ByT9*!`2Ge$mRt%Pi zmFiltQk@?Z9Bm8wNJQ+%I81>OFrMhv~vQq8n8O;P3)tW=wcm1=S6 zvAZZ%syX){Wm89zmX&I9hQg#+sWv8v6x2RSeH;x4ExTqynpM{K{i#$}|5U2`MV%$d zkh`Agw|*(rd{o4AmX{U^p#Dp#mZr4D>=P^1Z54I+=PTmYzi>E;(`OGXE7fKXUo;IKQ_qfKh`lRon^t|=T!c47MPZ^nDi`p_6FATp7QzT5)+M&RX*czZ9W z(aOsc&gi+p_~+WSxZ`Lx?|GDF`J+64rYiP!u1043`8tL806HS|FQW5|zUZjMoi){K ze_mc6T)w;xv-ov+t!n>%WdttU8+UxE?tX5UeVeiz2Zh(TWD zLfPlUYy(w!A7F7C_l3gnhl4D-O)axuX}Ev$aL9z&1Xb#S=PHciV@d&4>aHc+P1#IW zvauX2*;xKlO?N0f(iOP~6-5OPt33h<3w5Gh_2p;`ijibvS;B4818XL7!HvmVZkqe( zn-AeYWa0TFTG#q1{v*vBGqq@%6XMhKCQvp3iw}fZNgYjf3g@4k?}QP_m44pbL+ZPp z@W2c7#5jT9@x-czdz5@^MGYUtzRg!SixeabRLXqsaYIPBZ3(w6;kG5*wuIZ3aN81Y zTf%KixNQlyE#bB$+_r?<#x!vy+%^QNE8(^!+_r?Y@70_aX{yO;8|A3vcKKNF(7&6uzU)dH0Saw%PqHk43ZVmshgIIcv zq7>%4;4!I;I?F#oC~SM|Ue;r!5|eq7x%K$N`&T?N^j6gC?B~uXZg*86k#iA5>AVN- zSmOy$GD>v$jEU#_8i;G9PBD=!r+!CDSBaMo(%ma$j^`hGdiAcS%aD(3kjMny9lVt$ zd>yQSxV#Z2)QwK#P(BshFFWjNIF*shnFve2H^LBe+XKJ2ZNT-Y4q{Vvt7KzYvau}L zSbpr|(M4OLo|&#brS9xVfNEwRAwTmF+x_FKrJboAAK;Pil5KDD!Zp zZw~7Ea-QDwo1$*(b0lozQf$?EzTV*!NEsV$!C+#KZY?~OTW;}ft^sROor&fLOG6?YgiBn@={Kl!7 zw0Zu^zPps0@(J}Rmh9x6g-=_)0~c34XJYp99ODg?PCIzCrY!IhG2KEpv$^KvzMxQW z$qy$PE_>^HN%WOOUrF?pL|;ktl|)}j^p!+kN%WOOUrF?pL|;ktWvu`t(YN$(yd?Tc zqOTGCC@Bh;1`+u3KmPB7k^p#xt{!d@}-g5npQ`UH&Ci~)!jTMj+ zFi>G(A#OWPK!W&vZf^MYJFI6@A%D?tCZwkgL^Za`GGR2^=tFsr3o3 zTwM+Du(D7;BnozmJG%H{GuZ1)o#~q9duD>OR#VesK*){JN%E0vjWC9kKG#{b+nr_b zv0C|MF!JUCffe5ZVhs7;-)PV2wEM_VZmWFW<_=KKu}Qf(s@kZw!_ulXpcre`^D*OR zxxRJxk#d91D~?*L%e0$#o%6W2!Go8(aKZPuw5`*c76A)8zVCp+RIvlwIU`6pQKs$c z4%BKwni3}kJmE|EGKom`HrROl=a4aA9V8^hlb45#M{a%_p{G3a9I|hlYk}rsXV;Up)BZn|UOD3AYh_-p$;VZG26^Dtukt<_ zZ!bb_%>r0=(n*zMBaERwRrTzM53klmyeABDrFIzoQKBz*V(--Ja01+#5-vYTe4&J` zgl&1-qLG+r0Ny#5cW71zvpL-m+eY(Zsmn8-raKu~Y`N+5UK?|Ro`So0H+Ns{fqFw$ z0CF2?c>)-Gzy0lNEElU-D8&B|bZnmwPRCiZ8c{ru?gQG&A{I0=dkxgcbW4tP9kq$M zF-6uHly>_Fpinmnx5AP$&pQ@i26dz$_TY>M8PW;i;7dPUoJwS>vkp|4v9UM=6h}@qCYv#- zGgXS|XKyEdnaG7CkET8srAarvKyNcBn6DJL3v^WctMW;Xw2ZiMCxJ8(q|5+i zO!N4CY5E@C^s|6Rp|*UQdna62=KvDKAhpHI)pQEXuqO#c7Ihl!w@X}00Y7{kG;4@h6bhs)~{A1gvXvURgTnk9oFM zvb(_N{++9e!kqxQi4Vx$=;%DPR0pE^g1lm{MHWQKs|Jf{~r)*0Vq2T7<;$wV)CHM$G%Oo_cJ!#!?PfuZS;YI~joOUBi^wxmpg?#g+&HW z-WvoVhK+tDbCQhxX>xat27S~fQ;cWx<{+ZZ>+1qx3U5TmLLt?m3PPr_d0v*MyOVNB zJJ^+dE4PTW!*gXGW(jL5$}kIa)R^3GYwimu$@NB6s6qH*^BNW`!I}GpcfC?|p+638 zR)rrwm0&_;+{!Bo&CA#yGx(CsWK}i!#lpt4HYSbX44W0bkMP-&7>>h z1z(z0PZI1~*xr*|^r8qd@dnvQ+iDvI_;YcmCboJ62WAV?j*TbdMN;p-AybQSkMZyZ zy!1g@q8vNqn3aqIJws&%gio}U5rW!RK~j1^>qO8-e44Ev>Jp3Kc~TZf1EPykOHQsE z0ziiVh)F)K0q_c>Bcf@?7x3s~J9925fST#zEq4Q5=I`lVy;*n(0O|{OiD1W`t&{a6 zD=P!XIC`k_&fCSiy;W2YhKXQy!aBdXUW-n=lLl@kDVK>rJ(qzWOq59nxR|k?&PaRb zL?jZh&xMxa==rR8-M>eBWnE(0B0&nwoI6X3@I?TT&VyZegG;2L#)UXz0NgO?{(>e4 zW6Cui1@I>g!{6Yt>Cjg+_OHp_B3Ws=zlvUQezrBdj7DaFuc5X-wtcc?7 zRU@mT1l=mK)aN0Un!)>cr(C)qc7yO|;RvjV-4>Y$>%g^%b}}D>{=L|HWdMrACs2jB z^}RBmDTYsUjP=>Ezvp^|?!V@Gi^OxilvVn`F$K|S2e#d>x!z4zJ%#}oPD_0ezrs+w z)e9K^s9I|pg{0A;UHAi=p$G*bFnuS_@|nJFaq-+^XbrDuvoi!KFl(cLx)O_cOXCDN zIrKa)66XXmh*9li1Fl?5U$2ur5>XV2P{gCN4*~~C1z*O&NH$E(DM$ESP982}$`5f& zHmrzI92H<<$)|1b6&mBXnL}&DL%oluj28vasT!Co8`etpexRjdGMKx51lJ5ep?<66 zm_??Jz?i6_pNp}Pla?W2vktGR6KVL6PR7&#DWf(fcj)oT zi!q81pJT8?7*`Hh24y`?2hEh&sd2#D1ZK*3^c#K94WYXlMcP8d>15}}Y`41}r$>{FvpTUQW9MUjSN7LDejg5UPf*wE~bS9f~`>ywuxM z4@XyX(jpVpD}5A2h+Q;C%cLSB3Q$czLpgA>26!w5GR(oKF%dPq3dj@C5V<~p*YJ>0 zk7Q%MDwlW%IxaE~s!H%T>5~!GSLg^z` z-<~T~HIvb$lzsP5HwZxY^Dxf4o5VxC8u!#!6418Csz>oxqKvRIBv9;5RntdsBX*15 zd3f45wNWH9-t3qdC0+FWka(!~=xI*iuc2O`!bTt!Kxx%E*5tMR%9Eb9vh$#WB&-qr z!jHG-UUD%*y;~e81wp=GDg#c%L$&z?;?Gi{~=g!@nzo~A_YHMd_?-1=c9Xi?VqkH!M5SlZuMiFt@8tf7ZadIAH1Pm|=m`FprD92l>-) zgW@*CG)ia3r$6QSosYtJCw@-!TA-|T-}u-vb8Whc6m6w`WSOklMhFW=n~irKc-$!7 zKYfWmKw{KEYB3NY%%%ob&O22q05815wdR@b^CT!P?Lt$S<+vm;dHtEvI(SVNs({?3 zUIW|nJ#BfTS44fqIgxbD0anak;L^%3)nwWp=+y1bk?mDhMvsZE42dUtnY~4w5T+-r zdVv}!g2Jqk>V7DmEnr~540ly;6|eLH?(&h5JA)mjkZsydd5dJ}qtezS3apric4~wT z0pJ{FLNW7RQ73Aaop3;idR`5K&{B#S*I>ohbp3tiMF^^3r+BD$RTlB(W!$4}tGI5^ z04^lm8NSyY;h~>W&PBYP=q>Jp#BED?JMnv^_w8PRhDDEfrMEP2d8L=Tg-{x!!fg!G z3A4C5dwVa;8X^*?6ek>7B0n&pAmZyVLp&6ODR{Swk8_*9e1^S+YtT<0B+11ClKb<;v!r>(t!gv~f;ZCZjy7$Oa87ejM z<*JwuD~BtqeoyprZ=XmEapu5LWp>FFrXLML;yqx7BCuYAF6=GhiC#$fy;#pKSkEA( zgUD7R*s(b7KO7>tF5jjR1{)pCjj%M)qoQiq4rx#=^PYI3H`I<*jIS6HqHH*W^PQ;4 zwBajDT23-hg4#wftdV$cNdH6B^Mg(er}RMmqASO0FiIa_mO@xL2{mNmdPDG%2+}$6 z6`b)7=jXD@oAHWriLXNe8;*76oCne99p|ABTF-dTU%;Ic{B^;+l`)X9apigpVfItf zEUG;E?(J|ZY1o;uNQ=Sx^*IZIYbJ~MWFNyR@jmbLNM_!}a963qzVXfD{o;MzFMYVJ z%p$;>U`Ksrw2EUEWH-9;`rE{!pr#iL#2aR50`jw zRcce|y#x9_vJaeGmUR92CKv`wJosgnwyII?GyKL z#p>R&X0K}WjjtZ;l=THy1*xnHdds?u&~1WQGJ0*H-~)_n>I0xDcvkZB#FCHrbCn=g zlVzj`0sY)#5YZhhofag5#vyy=1mKu5$Hdv8V*^j-n1nl94iKs5WPILrezDSEKHRv^ z?2s`umOfEVf#GSxz~4{G1)x4mS}P2MJ%OA2NVVVIGfvp^_eZ=oi*#$r{#lQs-IIfN z1IrEP*J70_?C@mfoxT@=Bvil!oV}W;00863Daz>yx{K5s4D6&LlAA+7yAw+5S8iom z%Pz@L*%)uLRpnE+X`RTxn6hbQN5y6NX6=uNZVDV@Mqwo_QvktExj z+Q#PVFIQb~Adw&A&UBA?4vv3xR|$upaCyZ61gDKFcCjz^T!A&!nCc%vE(Ht^0e$b_ z{c%r*9PE&)f4U8_6L!BGH1#DEX#YKr@3rrv{`gEbczz{7`GFOMX+;(>Ki|9laQ^1s z?ibWWmd&H@rj%tzWVUq)x4my?zbk)+P9gD74KE)wagihdT(!jb5xzDZg*&}s4sYsX z7;@3Ht@HNUb$}oGujnR+{kaJbOfN4ozQ_bjN zIHVJFKe7G$M_sZN>L(w&rR3d0FD{&!-STLBU7-rV>}tLUg(btp^UL_hZ>Vw#At&~@ z8;9P~QcLg{&^lD-f>8Z(M#dmt$D>a7_KZmJeAi94qx4tft2dtKU)Y9J7rE*11FDHO z1A-%d!^{>p?Ka`y=*GujpUStKbDFAH5=Cx6?%v7=r{cZad?{2%dT>JX!eN{Bp>;-U zlUt?=e-p59Z9zAMDWv*Zz*rppZ|6Fa_neGGUdu`6@&{n z*a+-6rIUGo{`thLYfbvwnGkS|4RcUyb+f&-wk_=LSX!gT0}r10mAy2N3I+4H9P9lv z?{*cO;LHbK1ALANq3kk!mQBxSaj+CBNY+RX!*PG8cQtcRkrhJjw9Y@hkCS!Nd^Gg@%oH(*DMzq5f#S-yhq?h7W_#^@X`9%BE2 z*PsIQtrq&WI`8z`FG{(hU04;}_bYB4IX#J*4nnIO5E3?9uwBT$kv5iBQH~b8>AMhfbK36&hDm`#A zzqX|T{iPCUJbY=#$Ezy^90S)V_9*MP`ef%H!n-#wJwHhPs+ZE%eH&nkKfNwYA1Zd2 z(KUDzE#<~fS_RcmuT?RK6rArQc^@!eijdv#;3^|KccItl&|ozRc<1bSC0rJN`boF? zM(0bE;=!a`kKp4n5gmur*8-Xa=&iJxFRHT&33CNty1BQ!hffJyJvzqmn*zARNyjxI z`b_N1dPkur5{fLA$E!Zi!@B5*>c&;i*j7fiQV>!xij zkVET$<$usq7H5lYs#-v_Qbm+^MCwwG-+Rh2`6&uKS~C+gKsh=Qy+K==#G7J+34|#g z7=M)d_B9V8(_apyr>7ZNgh(IY71-8sDisEWn9%SpdHepgf0+Py1yGmj*R)PyEhR~T znnDuMR#>F?WQWHN;sCSGOl~5c%;m{9BW$3=Bza;0)4`d`GUuHjWLp=MEcB$y$q6kw z^n;W&>6-DgZlMRHgFve6486W)t)n1z3j&#$5Dk_s8O*b1DnMMNE-p{*y48t4?NgXq zM>PvuMn`Z_2)*^H!JxyA0j+Qna^3rAro(Pt_3H3Wd~|zd3s{IVok`kZo8PUP8FMda zdLLqnIM#{cSL-c>gYXK)@QwvmS^|B_ktK3lT^UffO(r*c=V^$#G%sz6rXS6U4hLSjNAf$Uu^(Uq#PXLyQzO6zh<$8atgW7Cfc$+s|gavGMG zwd8h))3N>QmwPR302FZQ+FrFqMs3zfRiWA`!t2=3qfg<4V}fq=ZTASzK5DKion|@& zW;mq;o0mmAb|VF*!C&n(NIBGwKd2Rzlty@a63s+>E4ycx5e~ULSI2Ug6N0vcvyKfN zZVJ*BC9Xa~)!Bxqv^v7XJRD)QU#{uYvA>*AXHY9819Ensy`$r*69OL+9yorKo3x@O z1@2$vs0kfRgT0{N0zSJM67TgWW=F0q3;kPmD1=FMc2=?P#6z`YveDzOLH-3?cVD)& z6U7;w|D=0O`KHl_DN>tV*-whQj-?5INjs9_}>lnnMOyw(&O3*&4s1z_S)U#h(Pp zQX+K1T%G0ahwgzbYJ#Qd6e4_8-Q2=7s>MDy*$a{3uwWRpj#af8c*lg42UKASBhros zu88a!!JR##Wa#lZ5i`j|ZjjS-uyuEN=S#kt-vx2e%DAnUAevk`S1sM2&4u~)Z`&{h zfnLuqFZUY7!Lvmh0%;({_ghYvzSRZf+r2mX;!R=$E_$kR5V%ZHazf!KzuXF$p}Uu* zen>g(eRwNOL1^#%e)pprR!B?DS+D_VuB=*m>^ofn4`_aeA|+MxIEF3pi)$L&Vh0Va zEtb~R?ADF)conSc&m5>*B(}ZO9;-b4c0-eb&6M^Ye&zd$72ud*duR>UsD4G2u5pY$ zbfmiZ#kE+s>Kz|Dd%SGSc9hrigVqO$KLZYZxUP40eYwOhZv3CFnhv|Jnh^dwRTEfR z;{7+OrrBG?%`Ym3JvjBUPjvN;tdSZTSYel-O3L1%s&>b_s1inK8tUZM!BrEl zN12HW#%A$rJu--fbxW%f`h3gPJ>O{Gy^K@s6GAA1U}L7a7t@NVv9)W$qlR)JzEXyx zzHl4Ppn&UNkhVq8sK`;qx@@qi$)nG1pv}KhCdP8gb_A&0L=R=`vQAd;H+WoteY?>V zvTki$zLXJB9=yN5#ib3pDM#~B8s@DU3xk{I66XlTCwZ_-8$l|wX6fgH zJ_7F;R3x?^XVTcJkvhwiZHo0$rHx8|8EtUm^b|XBN26Y+Cu#JdtT$OxTvA|45s!dO@&Oy$*v5-kzYUB9YrgtAT_tUT_i=-Ask(=e+a6|Ac1m~Gjr}qQV zbFzILOkrpWorC=!&5Z$-?ZH+HnvjwsDD2>5w`OL(Y_S1T=C%86%|{~E>eEj&+Jtb& zlP#GJcP+51JNW73d|ATy9u41QJks>8-{El1M!EJ8d-Y(JW%0~u&pwuSLd4p-LwJU{ zf6}RE0QQjTLAD&%rEPK?dfdWS>iVp%>yduk6YZfiQwsw-x_wuSM=WGo!OJjn6V!7-1Tffu+bj$#uaL zcmS89pMb3fh46768cdB|uSbSl{2%H^`oOAemlaWY$Ow0B*)2Tx#&6RyHQ7_Tm-u{` zy;-W~(5 z+t@wv<{mPDpityUX@F*?R$AxYz!>CGk`hRVk>e?ZKwaoA(=C*PTewvS5(#>(0+U4F z%1f#dc6VNnA3-*+4>{`Zzj*12)bYMx={Zh^4oQGhk7mQ4ir{+8ZF_5>!lI8vz^cWk zYe0c73qGC%?TTOH+r>v_)1#rr98lUH!Xp|8#|(BFEH-Gr;^HjJwoZuLTyrW-iq~N; zWw*69{1hSVnf856QEJ5T#1D_tlMEBh;HevdKM!kUz~*l9aTG+1j0pI+lY0cKGlFd7 zUXx$1sgj*+7!0FbeyozUZXbn-C5QVs`0+hWh+y3C8#m*(Hy6-uBn1Vb^EnTuEqo?P zDTy(wVGaU4?=-^{?!U3*T})QS?2YZOa<6?-!G`wJIG}VuRnh;0tH5UAiGF%Yxb-w> z+=Qc1C(trxB3qzR+ppLsvj`(>Opq+Pn}4i$9F@sMD2R}$j5YiF4fGix1wq_Jh2ODE z@gK1)@7Z!JGujbBj07lreU)^=axCK{{5zJpY*E2ZFr!w(Py92M3FGsRB9jK3*XA*) zT-YB;;4(JBNFe1)bhRZ%#q*A(3W1qjD%dTg1v(lbWv#{q9C#Q72CSB{-ir50_kREAGw z{5zFBO8K*8%OV~O@aw(J3OIV+mO`%XMoj}_swV-6|7!fnfb>fpi zqoOTENvI_jEYVfHO@!>OfmyI&V{|$>)o^5#N$NNArmp^Bo^jMH>m@1oD z!YVLNJbw%_8K_vq0U_X?a5bR3M4PeMD@-kz&;ysXFesNH zrR%q9dpj6PR&J&Mv6Q@64%|ZAZ3Lin3QCBesc|qzN?^I)rDK(HW7#lMk%r$@+&Z^R zc`9+PV`X|07<&{-&8TB$gb1+RmIJ%a{*E9!bm zm-~NavL(*3-<=F7iRG34>SQ)t$PIGEOfSZU12*JhEa+u<&oN^pXc!6G8UY$5l_CWg zo0v6Ned9}!K?ObpwNRpYuLWF)gEG=X)?nR4kXEFwq5uwMASX-8X#z}e2~7WV)#pJB zO;j;PLuVR*9{Q&n(W;PaV4%*SM#Nwwov!X7X#0Tc+Ru!SFkA7M7mWNKB&>+Uc|R@F zEUHvC!1@K&j*>t!43IMYXDQ=Vt!{v){-cz|>F*GivcTs#ze^eA#BwQPRKrh~kz;GG zeXC(M{48b6(!Z2WiA&kd;u8)_`0(=ooXQvtmJ_Yu0Cu`Km37yjTYa}`>H9xYnV_Lb z8Qb8ooXUD5s$(0LQyIVBL7d8r3_^8f>Oni}7saKFQuDi%;S0s3?34bfsTv>bKOZajr+j;tDDWgPmp`hmy8?~lzWggP=orS}UqP+SGGaeVJ*Oa_8EHhye zjAY@Jc&M%*<~OhMm6cF z0bx>pt5lZt_Gn~)Akn5~eIP~JP*^_9#q8r^J|Zh@IqN?y6*i}k&rp0`GO#B~Z3 zB5$@MO@O9Fn#z<`tK3Ma_$z|GS+YL4vYi+mOZI3aBlX`=1Mw+4Rs$wNq{cYr)~-|Y zo%7G21n2U>kV*3xw2K+BFE< z@vQ=V+{dPp20rdbu-~ch^>Mb-TJ!G{xK3Ux69V>QB5yD+1#&PXA>v^w^31U{jZB>2 zt5g%)E|3gNrXsUY*6*LzNyB~CD?p_-6SgnWE7SY!<{8&5FGo1DVKxl+L-@nVz(MN? zWEOs3((n~qe47n?Ul1M9L_(E|ZoHePhM&jX5f-&h%Tl=!^3!Qoe6IR5mwc~ zLTZq-6KZq_WgA2_;?8>MeYduBn2-Ul_gx#Vw~dd;Z89dvCEvD#+*!P&hFJrt#G{^b z0LrlbgbOrQ*z|nK3GvPqW(k1%(blQtTYI)a_q|5Wa`u8L2Dc&MtsojCNQkV$_XT&MyRr$C4*E*L*+sUW#TfK~ zw9cXjSchY-ZFEinff|o9!uBZ2naSkRdVEIMJq?{#W14GDI+8?t z?dw3Vjql?5SoGy=prwzxwI;9>Ry8benSnmM12QSI{vy+fW3ALZcc!cTBvpmp$+=0w zxb0qgQPhl#Zh zZT+AR&$ekLfn$;$Va?WTQsM8~ZMpw`8`e`j%IjHL%Qa6G*C9%KtTW=_X2{ZeeLenJ zSt`-H@{Fu`Pd5!*Fra@Fcs5QQ$cSAtL_tIvqg2yURee)P14cJ#ylqv@wG)oK$zwk4bIbFdnwYR!% zJuI$eS%GchS{7957{04F)}m{UoU}KKu*HB-c4F&+&!AkvgTwD~uzJu~!IR}$#_N6K z)g`WFm$x0Rhk-t4;U4~=>NK{EHwQm`xm?T6Ubwv+%R1YsXAEDi!7iP#!*lkZx49>d zWe$xvow!%av5b8X^lh*7kL6hQPytzVg7C?eaJ+6gmd*b79m^7}TWItFaV#r*-y@D? zO46V`o?ST?+;Bhk>QRqBj6SyW1J0rUrK>EwW6Y+*w^H$R&m07-&BiNFHYz% ztbo7dhtlNCEOtam4J+;0%JX;|6;wpdDQm>k8twbCgx*rPt)l?Xi%rG|q;4O2uO)A` zMwz7k!~Bx!fCFxf8LV7(XAk7vPRJC-y2j{jbd%gX%7ZZad7@?AcwwnTZH zdlcPl1qV0cdmp_+l>%TA33c;kVZ`dB}Eyh-4UXv?s^yazNka7>K`5vsO9D%zdnVm_%6 z)>3fFc&s(E2}ODlmP5>T1nE=2;~$yNmtr4kjH3=w5YcDsHqanzDjZ?CIAR`TJBi@Q z6Q!}vp3v5@k?qj|px;eDthhTqW%|g92K~CHed0{Q!Nt8Ll*DZA=_Uv?myHQ5Sb@}f zbEc`aBFI@c;ne{EY~?;H!Sr19vxh4%CHKE!5k$*+&?xQZ z^<2ZkuaHN9bBr*AB?bQe2-a#zh`eo@s?;b&WJSpm(AJE_i3+JR$&j+8;eK4A3?*;% z>Ddtf+$x^5aDl4zV{aCKUtG|f-6F1<$x_#QK;ZGdW|^ZqyDE7P#Z^HV;ZblqoU2jj&z6wvUAw4ffNnCRG{j$(>hB1}Q>G!AyDHdeyaV9ldOO^Q{dV6B@$z zMZRBp6d+O@VW@gr&L6s5Dy*C730e@pr_`Q5wqnh~c57jQ_Ct~729bdV#x6{qOMPb$ z0FC@W7D?} z6^k@A#cfRdYQCikJU%5|q($1e(bT?Z~kE`CgLwDwYQwzuRh( z)UM+9W-cUja!Hiud+X17c`LZi8EIHbPuE2SxtL3TO82&GybSo_C*krQ-`UH7EOC8& zAR-LQB7R@eylbN(FU`tb$fMwA{6YLYg@r070+Q$1#fA+=9rA04TFzwcpSuD1wfb4` z&6ynI$LAhIBtuXp?$DD7{>YPlblJO=B?Z#cbZ^(5bvxh=o8EFDLC#7XDaVHqBPU#? zX|0aFcxrtT%v<`%i{nuulCFd9phB*ZveFo4Z6G@>Q{nACe(LHw;p3Sb-arq6D4W4y zSf5^)6)a?nqYK`4caM6mL^y8?COQc3!%Pf2B|>Lar2aH{$Hg^g$cHN9ofM1pY^25*O_#Ji6K=> zAu@PUboM0hDyv%~DmuEO`rXScbAD0rX7U<|K9IWEpg#Yu!O@;Kf;sb{V6x@2sB z%U|~x>4$XQG~qQW1Yq%Vh17lx+c{Rod&)wjE#v)w)#u<-6TD79A!wSAoX|793nKSN zJE^#LB<^;FBA&v56BdKv2OHFhAzRTp(XTPY@rb|`)|%WEWoImpG1WA{jD(#OXQiNu zgE~9DkI0Q)Rr{8xo=)9_Zl|B#`>CSJQH}wZsWv$R286*Fd(GDaSVYQORLM9ClxozT z-U9#l!)b%OZ&&HvZ&IN`szTVkWUI^_@Q*q;O&5;=upd1q!FGKVsW|9+Ie01QnY;<( zQy!0K8x9HRn?k7#q>a)-a;OY96k?>9PNBJYp8E%O8 zGK(AwMpHz^1*F9AHFX8p1x@S|0X1vuc$GqYOS;`;C@wU+m-vkWcP#FLEKVvvv`(_< z#kgy*ie=Tu-Pa20j-m*Ff>&Z`M+0y3(EBQ&cY3yv808K;fS_hyC=E z8|Ju7m7YiJSMDzOYVos3o9f)E;u0YkGNLd%X>Cd(;E@=~(qFQp*!Rl0qw<5(3&Q#7 z11i|nKlo0v?Sou%z2scU0-&^jd@ zAjT;}f14f~wcrFL@|a|ESzcn}XL+?a$IGXA9>@Fe1^M8V1E0Q|z<~!QDs{bAKlQGT zAEc-*0wh=25SD#3U+NGqSuYcCQLhQ{HYm=cjz)q}w?}0i=({@?d{X-~AA{@3_Xb12 z;L`)WXQigoAg51mFsWmr&zKA57&1&$V}YYVF)HzspDflk748!4=-?6DcR?Ynw6}$V zYZ)>-QuhRm=E!MH9>_6X+Ny3;Uj7h?nB#cSIm?Vrz!k8qCNy3;Uj7h?n zB#cSIm?Vrz!kA?I&|O-%y<$2MVy}B<7qc_~mtafvUU@u(*x;LSgvB zK^EPnmf5c~+`oA^WWsEMDs{ng6-MzfrGP4R*AnigY^ED7aWEwg=Ku7?n8d-9IG7R# zQ{rGs988IWDRD3*4yMGxlsK3Y2UFr;N*qjygDG(^B@U*oMzae$;Jcj%=;a+=dSrMxr&Q4IzhDJF3;eHz;TU)TM* z{=M!e*Yk(!mk9^M|Ic+lns07SUyv$|dm&Uv!t`_^`_y+`O(=BS=gpz4i209oKX>At zO>c8qyY$l()(TiRKA7mLeTL`j^Vd7kI;`4W1gq|3Dxk{NgPv4t<7tAPu%P{N3pA!Y z(2Z8D!MPgo(@kD{vsY~toHi)Cqi}houahPdfuB?ODu}u2^(HN2>amYU7j21pX1e;6 zy0aqzs+oO+I0Mcg!JuvReM+|fY3T1s%`OLJ9?ta5L0wNtSsE(D8!n?^4pJZvZ&)gBZ(>SQbDqFGLQ2?Krs$s!D1G`GN(oKz?z9% zaAWe8o8~_H=0i9TS$IB)*0p|$|48%3Of8z`g!nYQ36xF1;saq;Qb$vr!ucoXJ7Gj} zrJpzVkovACJn#ZNF;3ujJh7_b9wi@JQNu^EZ}Sz-A_WNpl``LZ+z{R)%*#=uYOMHr z+naf5eQ(6d#om-RcaNsW*RT{DMZsS!_`vR^_AwmmgiCnn9J=q4WkbYgSos$ zs44plYLJ&p1-t?6;>3s|f_Q4os)c0&B@*dFd$M^Km^Qq%+h~bSGfwE&AJR1Kg~|7x zXkHF>2WxOuhIBkTmCjy(?gkb8GCU2N+^YV=@C1r-_l$>vv29K=ri`cWg1*(B1yWR0 zU%U(gyyd;|FjedekUytNW6+vOo9DmmyGyw#pHQD-$xhx`__XCaaPhzJKIfW~`+`Ej zB|n^Gxa_U(=cxGoyyl3A{{K)wnSTZq9|3E+cj%kO1uN%!T^GKTT}44s7sS zXZy+WMSP1Ffh}a%8}b3VA4uxV$;fO=I>=3@>UQ8wSBJCHJN_Ytnm?|Rxp_ZLMT#T! zKB&F^3%lu*n*g_VwNHhC5>ne<>b@3b?PXpu6X~t_)TQDi`s=3FQ>^}sh&o67^{e+_ zTI9@?!dYeM@Dy7u;kzQK4z#%-DoGDWJfgnhz>|R9NH6I9kZjVx*&R#+^I)bRND+NZ z-<8O${169dqgL?}9V{qd7%(Dzl-=q6Mg$Aq)rRtmb}BvDz>*3Jk(;#ttWtGJP02h= z_gUo2E4av{C>lp8ziu_z6OSNTK(1q#qMc-R;v*k>Q`MKP(!O6->1z9f`uQB3jlZx~ zftzAnN6sP-*l6UFUEHYOyD3foo;k$3jZA(=IFZcz5gUz}I8}CvZ9YD8>z)|!MNv1% zF|u>-toCr=PKy3s3M^hc6`9NgNlOvHvCnK2$5;@nxybaYap+DI$9au!WV%J15GcmC zw0Y4{L>deANjz_~z?HtdCqQPA3v*`UL{6 z-Qn)-%)S`!NpRHRFt6^a2-tqB1DnBhmak)d>GP(lDNoA5_P6J{`O`6V8Z*!r_*8r` zfT=7`TvF;2N<|m*l{`cgtI}e~8R+9fv`9y}o|Q?KgU2yK3S9XCPWskrjqYS;P#fFi z&?t0~E_MGX2D=$Kst37__aru>Glf=2Ehlc9%wLs94w^&YO!$%i!`^#;HI+X6+B>}g zNoYoDXi7H}r7EFUF(4{nnNaK~K^R384LuYg6cLmXs(`4WDu^1YfQms;sZPL#icY{X z2nc5fb!NWjdEWD`b=ErP{NlfGuf5h@_x|kbx?0f^gy_~l$PK2%adzbAtr!(LM3V_! z6hca`sRthu?>OgkoPeY}$43fbNFL&bA1*DFM5NeS^h+6pl1%BiMFFslfsHtap`l@6 zWSlNdT742}b}S4$MbJuN8xAZ8)|Vw9LJ506XQQt14P*&NbJ@6DviL(bW*d{}->oRw zAsH|uSsMjgA;bi*F>?e~v?*&2fSnOy6*HlSDTumb;?7GHls6Agl#b+u+8z<$e-oTG z?E)m)&~d!b=s{@lAastBut;}!6AE?i_bx)gC+Gl^#PVWef@#UYY>XE*DJondmw>U4 zip>_JD#5g-{Akl0dE5#4;m8oCGs(Ow<71{Mv0Brs3(y=XZ4vuNm2@uO(i!orhj z@M@Z+uQ2rr4{HxVV`;~t(6G>bkX7{fzZ|d&?3B!kqkk6-XE<;k4b#W{kFKGmz}k{7 zzR1Je`b*M}2(-SCfHB2~>2`iw;RT%JU@~NpfqVG7xCeE)Zb(ld3PZb>!Xbax4KEnu zJjj?E0=yObcmWUV*JX=jBIX6r$IGQ{y3iU;j(?&d!8BH(06$5AQYhjf9BeBE)#U`4 zM~1C|Wc@pEsN1s~IAo-6TMir+Wl#`66#lq>ZmBqdiekOOmvXRurHFVYD!m(TRDP65 zz{&D3`JT}3=fF3fSPV1eG#f);o(suB+LF$ix57(`ATAX##ax#aU*XPVIcYw?0!(-C zmL(PdPLMIMw)JDb`eZtu6no<&3f(kajaFrw-p5Wj+f>I(2C46{};yCEv& z(f-U(hQA^Oy#|7g7T{0Q;KlpTf^|dpQmMXw*9{p({vEJC22s)b(GtuHjZrD0x*-!) zTlZ7gN*4)RBS`Qq%fi-2psKL&sY~_|(uq*!-ww`z^61Gl$7q*f`!q&ch=Uw|Nz6)Db--?8-7v+8mTN(84!d5*e zk+8Mrhp;uYC_#JNF#iXadZg@F$i*@Gr7_Ug%DE)+wel)|`dYo3HGXyt8JF5A`H}2u zQP(h_P}DV~m9py2AM>dByK86>A?g~sHvQ}xlF=Pt)^KULYj|zBYsjcV;nXfAM5QPZ zJ%4l!)r$Y_8V>%uYe=n<1G|RoIv;Yw1Abk@_pV_`%{-mG&-Z87keRaFH4J@k4YZTt zjpq^lRG2V_pTwgxf<*mQfqy^|Gy%d#%{D7PPfS z4_8_V@Tzrq4Fdca0d7r^REQ#OQfc=s3D#jFHeyXjIM*$!Z~v*&@_>drM}@hb8zgxz-k3Z1+ zJ#9FGHNC*`m}`d>HQ8>wcRzsIJ1E)r-PihRVO{7uk*_rj3)Q>i@wcxvx*4NK=@t1} z{rja`Pc(vcNA?^=lYaB$fvgCcNZ9&r%G#G+w94#)NEwcvXV2u4c6KwmGdhAJ7|+? zA2CGnmC9IJ$?zFNbK@y|T3+oi4K_J8X3rUm8iXd(VPTG}(KF9d@7s=6xKa*DhD(%c|h(kWF1AY7Pz9dK@j7eGYQrWdCw58NmAb)g2Gmk5>dK-e7(^G zC~WOF{~>IJjcLv~z#|BEhiuU$;b+LSA+T=sB1jOsRQJ z=$GQk{Q>$~cYOD?#@+;dt%H$fz5+hzYxNt0i3~P;U9M0L|`cZe}HwvPhj0pGBrQBLdlhHU?O&9{fEhciA@VR3)If1 zAAZ&i)2EWgn+zP0>ks|e6E+Ig4GZt%x08#jz`Ei1q)uJc2v|34?|iFTjQ;JlY^5-y z0~^gEHpetAGD7|d915HVmW)&s@al68$a$S7GClm8VBnCz($^CO4&Q-+!_5AH{I;z| zA1QicFG4<=r8xzQ_k)2$r<;zk1lC&e)DS~b-%=Df%(qZ+JoO`RxOS5g>UM`Ha7ddh z2CitX2gGkUHg`WT?ZeyQ-BUi`ZB*@=KHdidhwi`0e^mMm1`Z#9fkR+G6gcD#DGxxR zubJ}m&1t;Y_h`ynCM&fv3_tY5zepSYtJ{v@a^MUfBw zF)nhp!XCp5wK*0NonYXQoD>}S8g#Zoui#&g&54|?)U+lN>j4-z1PHG`y-9xYK-nr5 zxoII_kUwr4|8Zf<$Th?`=)p8N6<&{bVh34&Q7qx4D`QVfawa|cAMC!Zn2hwS9N+Dc z95qLN@uH`$lX=cMP6Cl$mv-A59kB8w&|hhbTTFeA+x_rur|I@DhsLS|Zy~4Q&2Lmz zNyGaJ=6sp_2~E8R&cpG2GD%P3TVV^lIUV7^8sK!%m;5H*oIXR*aA(!#>=tGI-qIti&mEClk)3OU2#sr(%pWx$Mca>2BkW0_oT&2|ckNhgu z5!U&NU2FYPmc6|3T%$LQ{ac(CvtgXS#CaI1A%XoZ`_nnS1t5!9;KNTnuNH`#U8P>q6vC9}pPSw|`) z4zS;u9YT7C+SL2bcJ$st4n!%5x3xmhWNGq|mg+Ad{_C(b@={U9boSdlCUOO#aS}Fp zxc7;2x@8QPlVZI^IE7qRoU83~%}3fQ>#?d?G$+8HX44*#RrWeO+(ubGDg2{`Qp->! z!h=4Ay6kpcm_SsJx8W5AF~ij_sA|!eI#;R;Ln`+keYepH%{VYFj!xnsFQ_KT?UP*b zoAEZ~r3IqOP<&3sv;?`4`Iw9E6ZBqEZJkrO@AUam7qT$>ZV&P>ebmk&((voJcerAN zAXYA46Eci^&OH;oxxJ1G)2!D!*ruv~B6$VrIO{OSK`dg$9pI#}RYlp8q8Tk&tyk+L ze-JX;sdb{(&;46cxAXauBg}0B0~_iM*NS%x?Tydh2wxJS*4xGRv*jjvZxrM$d;YXn z(%;<90aQ~FsKz08Si-e6U6=HD)&Mbr-^NSC2^D zOEGXly;0WY6}D8L8!c3>|KrSwKjRdc7j>Yz%Bo=TkR16=i*YY{?_|*t%w*TgiHn6_ zU#cICzfrguD>PW_KLZ770mz3ZH(*=QC(Bmuf~jOIrB>K+4unwC5pzP#mo=I(GBeOk zZqM;$$7vRB-PUI$fTsQ;&9$oqF*A~cGcukm)F8TGL*Tg9%870iqCzDjXu4zdE-GxR zznw%WKMVEA?}Q#^B9<&{4jp))tmQN@P5~9KFQ{h@x}l`@d?9Gv8(*pR&OaG6uNIK* zD&w>Ql=Gt@Uyhfg77fL&hG%3N@L6k41*+RYY3+7mo|>PJ=Wm$u!^U`1NoQV^eih4L ztr|#2DFW83P8Or>UvaeZ12t@+bfYho9O>!j5PbM_tV<)A5K2L-onl|yij;!5XEMn- zuaz8gQY4bt)KyY`EQHV?=kh&>8BoKlSek7Iw8_5v!g)!m%m;z5_rX!sfl+qgx3_B6 zE902lm_64!2ud_{weIvJ#e1Hbij%+^I|HVi*Nnm+2Vx<$$8|l)6B>%%z{;_nUWIV7 zW?dG~dYTdEkTNd5D-$J?-JWNXQbgP~8>?#61W?5ggi!V?Tlw7Ra0!G=G^fwH@aX2e z^7u$5t%BxljV5_>%Je_0K~q|>&APmu@|rNIm96NDkxo@9PSy~rhS+I4L(OU?&^q`M z4tXH3W`b5gH{5;`m5)C%8vuKFq7@NMG=9_4 zro4s;Yn@QdgQ2WEfC~P&ZQn-0CF%^t=cpKusBHoX8mOd#X0ZHJXIfG zEF&uoJ^lc*%yQQ6mZ@QtMQf7Y;^8c7izzffuP%++#BMvUSfM_3g{*j-2Ozfzq8~+Y zc6uAy5X!$npyLrJ;QDI}9gK{o(F<4`i zuJrM^CW(%QHA@C}$i=FNO}of4@tRO-LS2CFYq< z-k5DizE=-H6hHh}Vw?Z|I9wwZvCmx09SMI)eaHaI2QXl2DLPupw0akCo@Nxd0TKF#NfIcY}H#^WJ!7XyM zbeq!)%ZUwL*WZ>mK-Oe(JvIk*ExTUt?)f5ey*3HyE?$~;>*hh%tLBXppzHNq;#_o| z5-#n=spuVI6xO+Y0Wk-nc&?!9^_C?ketexs^@={X&-&h+spHh-O~kGyvLrWWPT75`V_ zhgtbfnMdrH2HhLT3m<~4p$AIS^5kRv0Deh=w4ZralHM#35SwGIdD6QR=?puK-tcxZ zy}hkFCH&)@s|WIRjMP!R4yn5Xh0&4odBPHozzwNB@nzQ;&1&jci|pLV^WcfSevl~p zscF8}u@_jBB}di0Y!j5K)d1ECNy=8z^IJXil4__c#duMFg{QY2er8r;spiwy()6)! z-yI{^VfLM^+)K;WmVNgr&YP~x(oIu zV;DDJn}fNU87%`PuFZRJwDCpGW=m4s5str=1B)b|6$rQ@v$#?1=`LMbueFVc`%y}z zexIHlVUQaUi0!I-q9_(SU#D#z8G}rwEd;_Y=CK`ICe&W9|7+#RyW`Bkm70T%l~(vE zuR%qWO=ra+O}N&Zs%q6ntc07yQqy@>UTT6jMwWb5!Xe?%U##$p#dDi4b-2u-w=>j7 zUbCzw8u1V_^4hno{Rj&sto0}Fsm9CCS}StqDMvCZNN2ZcbDyNHNP?yvd8eN^nR;wc zmZQ1_;_#U5eW*F5kMs5h0Gk$68YpJ!c<%lhV}E0M=EP;GVd&^v1MwwOnE#QEv(joL z`;MIXzBLVr=iip7vVL6^iFy7#Lr) zh`A$qafh*$;NekeR?5z=pD94;_eY>Ah?#mHHDw8?6iYds-5RR6y5kmIp;pC3xIlhk)OCr16bQhQ+x zud|=r)Y+F{bS5lgYry^RbsE9f?1xcd30XxO3R_cND%Rs-rBpT(62^^J>tD>y-^@NU zJmOJ&Ms{zO0B>t?GidN=LYUA*164r8BBT}gE(R9~hDdbcsD7=~5$$0!?9fSxJAY_T zg(6>=YgD(U?-ejuD%B)(huA{L^nKv3ldU{BASRGhHHa}~9k!IFN@sL9)|q+-%O=v| zAh`~zx2NeA-n5nWH zt|SP|ffKu9fJ$`Vcj3d*pzzx6_qLG595GkPJy8!-99%{Y__8Kj`xZWY4D{Jr!q2cC_E3^pR8mh0YIIdX3{F| zb&}_Q=UAQ9LK|^-CbuXM9e03qY9U%xEay(7mu-x8dbH1%{`lMbW7cl^N>xZw!FZW( zj%&*4I<%);Pm;{nda@8Nd2ja4<|;y*8i}ev<1FvWUG0f83fsV4Sry!s0j*n1m%T4k z@O{EPGOOuJ_-Quh&>q9HzNZXrw|q*Ele!+i{!brFWEBn5$q}sVEsB!}1A-%5F?SY1 z)Lpy>2wB(fn_c&@H=dl3zfZE$dFuz=yAT5JWVMSuw1I?bF(X!-z+xx(iN4Soxz&fd zbUCe_=X^cnN(2eeYN5$40X=e!FUBDVa(i~1%<3dX+2;FU@T0aXyPc3LpdKl|QJAGV z)g3F!J-UMiqz#@}`o^?8P9ky{Si^lxHJ&yMWoIM)xjRO0+Q5CGBP4PcG*RM1j?t+K z)fE!?(faPz5`LC~+8REf6ePqM&!$7j>^ExN{|Y-wCTNcSzLG%hwA8;Vfm_-~hQ6l7 z(JxiRB8Ba2*3KSF%7p0w4<7#LWq%pzRz&NLFOWPw6KSfF_8aMJ+*wxJnhi{tq|-~J zz$FIjLP+p1-W>Oo(yHrL1ktj5+W080bxk?%h3i)$+!t1lX?)lk}qcO zbWZ7V4??Nq z*LIU@N~}`TkvfAU)OG-h^-6~)#0#V)yeX@nh4{bm9GAbbTLl(KOxPOu;b&b^?@M^x z0OO9bd!CPYSZ?!eMuoa14}S>!7N*?firE2J$Z(^Up!ZGpZE$Z&QD+q2wco{1;Zg0U zw%RD6yc2LP4A!ZAQT%VzPE?){5gf0AIoTbWo=G|Lu8IM@NYj?k3_(T|4XNP{+*9%P zYdX>aX_9CFXKb!O|G5uLG-Jc_@N_%t?>Mvx3FrZew+A?N)QAqn=gONxob;!@BeM?S_r%7?^a^ zUGG6#gEr<}?coUI$^if)4a6FaT1%+qwy%gSXw#;Ri_i51!Cn|(6J7;CgIT>t*(}Sm zpO`hiRvqHKkk=n;38YnMxY+shy{Hxyj1lGJhH`QmPJKsanJUvtD41$bP{qt~VJG$I zI3JpytRIuBbt>8YH>i<@eiH!RMol~f!wsjz{^vP54>lvSXiC-0%Mm~AwX>&)3t5u+ zWWao4n4f9a|a9cy}ns%z00Fmq0-b&ZUX~wiLWD|)I272=~2 zk+2tfm5xfJW42Jm`-~B9IgorChZ=~&*CqNbnfCcfuNEED5fR|t(E;3K$a+4`kR4or`PfR?_ zkJ!=yAPKkyK@y6G7f3~UQ&3BdHpdw_g#bwZIZP89^N=i_%RrF&Q%0wa&4@5vLwQpM zq>zHl<>03VQ$JMTDSY%*o`IGC$qxM8d3lrerf*mE+Tg5;S`;DRCfU%z2BTiS3Bj!Kq5a#N?A5V=Am`71J)ioo}zzGh?6Y0|cu^u|4?Yn^Fw z{P<)dwyaK3U4W-9z4j|Ph9k8?an6Xf6o3FgifLH{D%xHJ#qvnzvM9Z_fFvVh7J%CG zuE?L8k|f;KfjYo5uB0bduj(BEal-1htdq1QjDCBp^l)iAI1OsQJN5(-41&q zz~?dICaz1#b3HyRbH>ptbK{@j?FAT(uSiJ&{u=eSrvyYP0p-p^6WJ-kPNjyA5Non( zR}A9$-?KE03C_~ORk_o90BxmbZ+a`XE{a$bO+oMzj(kb!ic#!8@s#T9mOe zXA>JVU*XQBahuol6%=qBoNk2@*s6wKYs) zD7nx=F8B%uolJ)m($U!;e{9sbpNoG?)a=NACTen?^PCBo6-98!qO53*rdKZuoT&Gj z=X}qKW=n~!IUsM~Q(CeT;y8@|_ew3b6jAQWz#KAlH1{BZY0+Ij)1qgZODvgLk>o-! zEvgeCZ_uLzrbWX63!D%(N{4KtVShhh1}@cWii{Sy<+=tt78xjWSzS#*(vPv4c5zsU zT4ZCh!MG^JNRyVF1CG^%U=~44f{G0@zx0hOPGb^}euOOY;5y@y?n0e~Bfv2~$TSi{BZO;V(3r4Hs| z7&?4}a8-J^j7c|(XMp`qQxpY7>%AQP28v-L6kr_0y!0xxq`vjBU18d-Nz02ht=eLV zUeo>tf0PgmHbyPGHO~*iRS%l^(&0Wd+~&u+bs?FgUg8QOj>@AN+d^`CMXR-jy!Z&S zK$ID!9>|Wt;y9FCQ_*T|ducf{%83vU*A{q|l3NYj4NMj!FNa%S9i!HXR%>QmHn>`| zYo?!wDx(U|cjtnuHSMAzRm_-H*G?;qWY#B#lDbMRDWp^G2rnf(Dhl^#`g^smJs?`G zudU2&%;6DK1<}A5BN8XE}?t`(0~(7^T4?m&C-}Vj9wi>O+uz< zwWdd`41tI;qtv>;Go#E4rLy*7`czmy^`>+r(R?9rMR;FL7)MizF z9Z8mlrm=zN6frnZ_@gpPxfucQ{;7;I+D-5Mtc-Ga10z2wqq7IWT-R-|GJ5);nZ2BD zUApxYkEjqzI(oyn(iZ6SZC+ljZ$AowL{fosyf!^CaI|jr;dTY>r~^l9TT5`XZjl6P z+MA*K@&F92CfXTm_;uYoiz}khnt4UpQpw&I*T2LQKX{i1uGS7MN?WzNcIilDpX?De zMtKL0x2Z;Qp~2O6B4}3+TUQ?h!LKh|?*?~i5Td%dzx2fVG(?HaI_B6%k|||NLL|Xd}sml!ZUhd zrF<^_>!SsRZ-MHyx-q8k9rws)&st_TrCre1+r9Fd07B$@M_{ z5WCz&sZI*3A#{}?uH74DC2_EOW_xKnFns!ZW0d#g`(!=E!b}_zP1f(h$(j({wf<5033@dlGDNbOb}wd9Dy zglFGRBH{(uVgZKuY~p&)@El#N^`N{J5#HcUY>$&~IwW6eXlrkqpLl=IX(M3Wu8j9i zDC{$6tT5;MXuW1JRnaVi_)Nh4$SxP)%X~LqH++H*QG7{!nRIO8iK==16ds@7435_O zp1ri^yqp3@>m@pD;;+;lsjtK~6z;r?Ph!Sdc`C{2iW;Njc7E%GDKuuSDrr*+SmLGE zfQ`|ZT`+p`+n5ijUlLz|jnSZ8*YVCnSJx}KhrA*BIiBw`D(-V(5gUvM*dn$#mNuA@ zVQt4j87KzF3fNQ(RBF*Ap7#9m6}&xtUjY*@j(Jux_M{<|X!VfPECKGdwdlYRD@jy* zpmG}1&^XH+T&=y`>n$l+W#ngyOpN8T^c-g0umEd8c~^EXwQ*6+@kNPrtAPgZVRFry zi*5QN#D;AFdgsB>n%cX!O5FMbVyysCK*qugOIzr|iNiX#OMe5~p=Rrh+=1y7$pQ5x zDpV_TpdMVUfqvB!fyuO&Q?i<~T_9o&MEhvg<>x*NWv z-hcZ*jI%(!zx7Xoo3-|&B+4r4Q}T3cxg+xCnZL#kz5s`7g+BatcBQrzaeco~JG0hz z)A|=`cNa1$lYaY?*)zMh7yF(?yq?**NJd_r!f*0ewFHH0(A^)wR0;~G7OBV+mN}=t zep+u;XwOT%qG+heGzt2HAWt9jO6}dUd7gXb_49P==G%j1>kCOrxx?(yy)NjfC2yEb zq4?_YUa3}T_wiPO{}dP-ebS+Mj4^MOF?%;i6dQ%F!P1k5Vf^cReKSsp6BcL1EnhuQ zjz5i&(GSdFr{9*Gr-_xCQu;cRU|LzT?EZjG++w`O;FAZXjrNLeR{|@`y3`Np?miEn zj=$rUr(-es{A(rb130C6s%#(o!radakS9)~J{8hc(In%PmvdL_7^6E%E z>b{<#2b+^scZW&{Djx`}OLR$E`M03(vQt?|TKBu+P+`=mmexAtk0>GW&XIw_q?M)Yl^(W|wUuszBjnn!tE zIL6>bxZ&LwXx~x)%1^cpNjxt-y;m|c??`upZg08H$?=OUr6#qVzoYtCf{JzO(f8p~ zyDMYl{Ab)QKoq_N-IhIhKG{KWvY)J=zyxd{249OA%Bv5T00_x~m&sL!2TNOxwoQ^d zHsuvmKwi^F+U{@^nkO?7y;W_@Bx3KRpjl)aOdOo-vM#qh=lRE_l!VovYKqW_+a_8s zEcy~&KK*7m-lpuZ>)Y!qqV-zdNsA_E!>MJltM(3t8F^3ohQp#&xvk}B=<)OL?GW74 z)mw-IiwT(1`qyDBz0%aTF-H3L2oJ?i)5BlAsjq=h%|~jZra_H26t=b2XMF6`n*Xg#Yr5AVGjF8!QiSC zaZ_*l`-jrE%coqRXERoE!?`gMtmu_HjkY`quU$gNka;0H*)iRHj1dAhOi~Wf6t;vty1TYs#(GpAPP+h>oW=%cy zS>xW0plPy;KJ=WF6x3d8X1A>@skn@rWA8S5hq`0N-^LxtqMQnJ3Kc50H%DDJ!m?0o zOtOJkam+gHQlKiS1h@HPjCt*w)Z+^>+lP69GXkB zo>L=k4A;b)1JQ1P6H()SYxLm*j{v9O5^YU{+HoFimnEcJ58a{g>}$+Q#n0tvr|c8( zkgknp(avGCSa@R=+E$mm!?$6-L^p(msCWQq1Iq5pD|Vc-XvKbV4(;_RM&`iocN;_b zVwt2in8t3nnP*K*d2i_wOi>W0iKM}D3fhCuP?&Rk2dv#>AY@Tj*CkW{$u`~sE$D2Q zku!)lncSrGVEmDM7oVkbvbc1+FF@V{Wtmks)UVhG@!uPbwHE8GoA*B)ntQ^D8oPB> zH5o3ZOCB9hm4bB9?cHSscw!Jo>{TFKt@-?|Km%0-f^eP^X1)`hayVp>vE}F*UjP+2 zff+lSxGCr2y1$~^baT2pgtgMJ`Cav5vP{UeELhZq*-~a|a)MQN)LQB3Sj2u;23c9y z^19|ZWRa43`WZ*uRuEO6iep+`=i@!*9F;=7LlS;#;HZ?gH%)$YR9XB8*8|$19~s3~ zLVntZ1`WdE61n!wjZfow8%%scc`*=gVbT(lqx?d62bnh;jf1Q8_#Kb4+QkImpEw~? zj3c&66!-&QD)eeNOzee$<*Eh%`ULG5S$I!E8BXPYK*VasACvg2G_6UCg}RWx00bUN z#3A|^W$)FJBu=h?_)kh$K^;RjLlstz4H2PF!#RpvVXv687zWeUxXrt@t% zS*qC^+aczcFff~|A{Y63Ia%r^+oM_=7Vk&fBV09@D@lqeJ48;-*yR^)37K)ZXTTb8 z8qZ31jZ;Zt4EE=dW&i?3_D$9o319*G%{=C^2Y8ay*g)%t z0d}fK!QU(QMxreC*|>!>*Jw-IL`Hh^#IHPC8C*Is)*jiK+P|(}?9n*Jz<@^vm!9=k z)k!NjcVT&Nj>eRdSH@EkWhXDEh3XQ8vWg#&A0~VP@|yL?vvQiA0a41vMJ3+^QJR!~ zVS8>Ytk;coJA$36M1izo_fIEpRK)_;H=s)d2$Dh;qc$Etv>4vmSXes zlRc0tVan+eo6A7Z?tUeWfN@e*!-thnw>5)Ii5#IFvAHeAE6W&x+2~{$xR>pHasu^E zIm@b-(}=1cgI0DYp*};P3UZI!x^tHMl-S3lOvs@Ry`~#wNaMf zrl^KTpVEwL7`c6V*5X6HJ^_|w8&QS+qtiLtJeMB z36%aCuYZl#zsBodtEyb zuko7rYrOt7UjG`ee~s6_#_M0>^{?^z*LeMFy#6&_{~E7tEyb|Euv@Ia5Sl zTSomuUI#M&A+M+Z8}hpAzmV6lTatj1{~)jD*#A9wE&hYNrvINMucd(#|C{8s{Qo|A zE%v`6uN^lYzj;kTfpy5bHJjx_zUTh*>+^x5#}>Y-Sq<^DUYD>lX1kAXjJU2K!OD+{ zK9oE6k>?)wHS|stP49p_*4m-!Z58WsG%VBh%F?Rh^5-gBqY+MV5raq5SbN`ATGeO= zm8NkKcWO?i&Z-@rC$Md=kj_NVeQ&2G*bvrBT++LAOjrBWhLlmbw!_Qxw+9=Z#}zY6 zMjBlwnw~2*#$9yU!t_|H?wS0d*r@dFhuc)keFxm`2$H;f7U`|}E%Uzb>>8J9BgERR z`j|NiAsU@%2ZGv{?sD;^IcqM_3TwBHTyi3y%c+b`%DAH`S6it}@;1z%!qaDN4OLfr znG0Gijhc8yg+sw^n|6j@LK_!s9Ueyee*I^QlBJw)^xw*>! z4|&Z5$!k-Pyf*opyguuodP_uJQ+K*&JpE7dnyjqpR)qDq4!3gR`Z{a3LRRdy*tgFg zqekbUv)IXUnr)bacNOKA=CZ3JHF?UJ)3{{w1d?8-pX{9JIQC+NjdS?8{O#VcD9OJ{ z_T>3vq$=z-T&Zb0NFlF?qIDkH6M(SYks7uLP~CGT@PB-}lK40?PO_rgQO2FB%m)+^ z3voH*oVS}aXFKK9)6-l`JyX1@;V=b*YV-9X7EA#&Y3Pn;F=a*6 zi>FZwUghL_03y4)6Fy@AF+49B_Zux1^5^xm1N&$io|L;AhWRFkxUaO7S+)jF$M1T+ z@|U}o?$hYRq+VnL*-b#a^o61BlPPo{glgTG8Wm$KjbGi9`Q*i%M^c?x6tr~KMk_x( z!O|3#aWap&zQcAYx*@mH>%doM%!^pm`fIs%f{vL|}W3xrlyyPlWc&p{Y_zxC9130O9P&voGxs{gbXnZN}rtQ3G6 zk{Ux3`(MdxZ4r5GuuNVL==#PqKTcZ1`Tal1YnaSxW*TGS^_h1cC7r^!=6Tb$GEMvg z8_GG!o7uJ`e0PwpF%u=f02MzO2%rP!&@)pol+i3eXasN^8WVe#{6;z(0`XkBr&c$` zWg=A>5?jz;io^vd6{-2kEa{|>N5xEK2WqFbW;8CkX9{Lo1QBQ0DBae4i`5~<{dN*S zsk3{eS6j88#tI zXO2vq*uHqxD%Z4o5=G6AcQWN{c#h6YRSCvEzBU(f-AKU(_icy%1|0*Ko7Xw1+ab#Q}8agrd$BeVJR+BQl0m*^qsm zFZ+II#d-dcOpv?uY*#$(J-ufw<<+}Io7xN^U{%|&LGF_Jl9~pi^_>?Fi8T&L+q}a; z($wIUy6trWeQ*y}Q7a8r-Hy;OwX>>cPoCmTz;3gmZQI%K4YOuF&N=Z%Wg?PFCzz6Z zb5`7?)^fTOqGhsl92M*=?Jd;Dp#-JS!nJfP?fN+HE8rmQ1>&+i$YVt&LVGUi3@pW_ zKs(F2?2^vGJ=;t$_xByaUX4mSUki~R1P#BQM~EKw;#ORGyTPo7nDs>=-kBGj^KA-c zeO)EvH&Q3ct_acV;g0ZdSchFrh*nSWe-fDbyW|vizqIECs9>Z`ZEBU{S(ac|(6fYL z=~kd;D+FnOw6RN1z_QDck2{dx)$$XNaUV?t~RA4V!ZxU+Eb%$4$_GOkn_eRj*xm3Zt=pTw?<)uf!RIs z}(N)of>X=_TmRXm|VVZ=6kTEVPu*(! z7lhqT10ie%dcP2YVq&gm+pMBvDO7|P8Se$aPI1MaCD>#L@Qplt)mimR0@t{`j4fQy zZ3TCx!Ni5)hdB{X>97#Gnkk@4N|)O7Ji?<6A#MYeW^So+bX_IH9%ZAmX@~*3%Vv&5 zBv(tCXE!fI%W{#5gHBQcq#psgiiw;xM(m*@4+Ho;0x=iPp1VeCL=^VgWeS@a2U6Gv zzEjxTxE);^HPsMv}7%2sn=8mDy1){->U@W4^Lce(ZlC*vuV70e)MNR9U0q-(GOxISlWo z7yR5LvfLN2Z^|V$8px=&;W1jdp2$dd!&QVhrna=M( zGq|;5BLD}@;M1CEA~QH6Lu3Z0r_X#hgSW<_$`2l;z)~peye065XfCOUvaAN@gw|fG6WN*s$AFOIDT3VY#Jm8I$fl>&Buc0Outx+` z6+tYTDv`1zNC{yg20lx+EF;;SAd*e9Fk+lnSSGU5%<{i16WMFCrSL*%Y;-0Ucw9CG{!X|OivO-Keko$}QLjnh-eM=i-tg?@qN7Jw`1bAHl`Vdnd z*)DGH0brwvD1q=| zoa1EnnK^E(LF|Zunk5o-_{ak7GE4H=NMYHRE?5E=kmR8=lMsyrTq6&|{et-pWhc6b zpzL=f_@dyVZjpKMM8NH6C`bfL^2LpL;)zw_rv>;*O1=evMGAl#21JsATEPVrs1nfv zpp;qBIgB3x5F~38Wx8~?iKIJUXa3(%wi?J*LnJ8N?jD$l08+{q0}m$kD22d+5Um9qoXs{`d7b640Pl)c18$j2ZE zGg4GxG&zbAdB?t7^>Vn1#25$lq#34{oVQ3v8P8w&&SHBu=+;59^oNOY%Ph8$Eu?Zz z5@fMWL@aiph{a|e*rZeDONIGRBx?6YmCxwya5)fPiw~(=qe&^zodCud^&$kDTOSD` z*o3;Y;i`C7j)=Tw*0=M@BB>3Dp`^}_P`z;}zOU)23&gWJa0~~9p~AE&b&;X4MMhrX z@p9wsSVaNUfm!dvkX#gAjbKx3lS%4ti641NX$+*1v5{F56i$_^p#l-q#t06|m@7Wa zhp?#FD8r zqyBH;`pm(SAHX%gMFd<^D?s47#1&RB1kY-^@p{%(s$Z(aX4Qp%1J?x358#^D62ZCs z4{%LrH~bs8rtKbKqiE@K;Syg)ojO{4hUR3+Y#kMNhdBCU$h0DxGV$L(Mm$;u zuAf*ceFv_;e7SyzQC~WLpRluoM(LMc-$mT`K<85ZIuN)f_r`}J&7|OUBtDgpFRxHV-mHbP(=i|20Qc7!Uig(Wh$AEJyU1`0@sW> zN@V~2?ROoxZ6I(xn^I`1c;Q-rDcYu$IFA2Q%4qc=d9&jfAJA<%1`z?*ytePaweD0K z2wXoI!JFLFQWa21bwHU%-lLXnQI(ZLI8ED;@*&Xwj?#nQN|yx!xmw z-j`pg{e!vAd&b~Y;BsXvA1$zb^Nw7To@QjR>sE<@?l2e28#5tO5ji~Ep38_~ z0Gdip%K@HcwPR-~X~6>QmsO+h5-@j0O=K(YS!nHWCc$gB5p4x{IGglG9e#GbQL_`= zhYRbpLnS$*e>!9{8PQ-hQ`Ly zltCJKZ=?~hwlCwO6AA-j*Zkb?*tPzyH1*01LZ9GjKzccQ3~dL!*7g`bt@v{K#ls^L z$x!p0w|LC@IuN_wbL*u&^X1I4EBx)!wlv?vuW%o(%T}V}=G0z}*GYMhjtLE{BuySl zQnuDb|5>jYlLw$jAnOQ|Yk7@jN|T?$IeVT#VVqaEL+R|B!;Rz8S^7l%&^Hzz-&Dnt zc3S)}g)?tzuC#+ka36kgK+9BSk;&tc0chhZZ$VRd@+-_NHT~JQp{;uFNYiR(Lf#P# zB(3LPp`=lQB9g@+Wq^peHh0g+;n#|oYv3J6H?3|_&B3covR_Jr z{xI2kOmIql=k~RaALdCC#0#rI>BsY3G}tZyrWg>?dvVaE_YD)S=Us_v{Q-v0;bK-w zmK&k!Ohdc(y>d$c!cn}TEpspnHZ#Y-sxFN#@>i^S;L)ihvumXH#Hrg`&U~a}p)=hT za#Z1j1aSkKqLxg>C=c$K6Z4@yb6L8wiJYb&colE7=Ew&^tKC`By_)Kv`C1GDPzQ9G8`OL^n;W34V3b<{_ z){qx!_2QYCQNb@GL&sy3g)=PTHKSH@cJHE4diTT{Dh>L=cs;@YYk7usAni{2P>$(? z_u9VjnTla@4D@K|U+5^gY{i!)mTRkn<%@ERxohS-JD#1(mHCH zw2o>eEE$SW>w$XBV}iM4=xnE(2(<>*V1rzTVLV)7Z$|jWcQ^5$q-qST+w2=N9}}Pi zjSWIIZ@!@Q2VPwS;?)Z7r4=H)T6(Ry2(NZvOIb{+fp~RwwEDu=`KJ$Gy5{Ja%s%4c zyRGv~pIc(j8m&(9UM%?i;3PZY+-hZ$1Jk8f5okS?`QryX><)B8)IVRgqzc=6p$46| zEXUV$-y&~#d@5Wzw#`}^Hhw_3+Aa;3R8T=>`POLl$QGFI``WFh9cw=))Vf)fF41G6 zgWI#+?8%H3$XQR>7Cl)#vZY8f<&}wT>-Ouiq%${Mi8jfyusPxr)hgst&mHFKC?*h1 zI4i5JvF=ItJB1#n49peF5FcHs(~rL#nqK&texE$lr(`Ssv=FWt;YAxw_f|eDw@VhW z8e{!#{;!(EV0cl(1LtV#Z|fOKMjvY;BdXjl-v43%Qq}`MDeE&-I{wzhdIorPrkvlT z_9aM=REYS7xpe#7cgmW?U%gsFL|G4fr>u*wCW4f;4bQTtoA8kWQr6;HZ4Ns2S=)P{ zlD{P-tb)Cqsl1UVqO3{laU#l^l&T6+){sWoAm#y8y#i9En^A}^NLd5zF{@5}YUlK1 z5B3BZU9)@_WN)Q+ot_6zne|lL<_%ejlG&DhWVaN7N+|0$vG-qNcv^x**8qeIj+;(@ z7)-EukT)wuE1aOS@2=DS{jBL_QsaP-#eMQAWIkQZr*PmKahY%UCK(HI@{qW{^~> zQ7UTGe=bRso`4gU(xm@NlbD!(&;oUc$>rnr^=~eRN z_ef{m8@`fl8TQpWm7R^MfTXi;zer!;Wum8vlM{lF&~vqB*^i2;C%}qc(rYdJmXW;F z&TZw%gjLRo6{-25&(US44)H41E)N6AH1??Yuy+Cr`KJ@!Hkzrb7}VPNPbZududCG> zeHG@vZP&s|1+%%(ryHPi_u*X6`k$x3FgZsWm71KG>a#KwcO_*Ni40bc%&5+(TAN++ zz3&x)>fp4xudnh;T@T7-ywrafJ7E5LOhU++dtq?Ge?RYC#n z#3>uVQAuO=ixW-*Ap8|%v7<+yTAh0<1K}{iBo|CIdYhim&kRI?-L}B(01z_5)&upK zxp1yz8D+zmfmtb&l(M0eeHnPJxLPs&wkMVY8SUD~2w*)pm&2l=vQJ7-7 z7NoS#)@Ivf(?kb1B|LjfXAiE&z};~)Ln$1jp_GxnuA}3~=LT!T+XRD6`MFN-NDjyU za*f(rjF;WwQ`}kW1`R#B*51OslQ{bZv1cq#t$*TT?Vh)2T;&^|FkUiuBo}O1FSMNI z-(FKH&Dny|U(4Jha_Q996nxA;8VU?C1c2pJ9G?5CI!FJf9;~p@7j2=ydUsbRP-!B$ zt3I$c;9wW#%i6a;0uxk$F<}sbUye;acTGd8lZoPbn62u7L_*hU*K_+Q+F##+PEc;}weT=~SFGm+GSbVjuY>8kd!ivPaHSeu+;3) zvHp}7yW-pOMuZYmR38Un6y41fRNtruW1MfE8H4T8G~0R?#L@AsuV+%9=>Fj6tcDirgI>2U z+^-k~9ha&$Y(IJRM6x0`W7N4w>K}Q~E8trC5Xo{@o$aJV&o4`z3ml*y#?AF0@Hg0$ zRH9v@0dHfgSz|lNUt!2X;ueiiwJ%*mxHYSsz^E7V(h+suR}s6)0hEKXt!rII>%2 ztO5Ve2>=M_aDiW^2Ph1n98X_470Cei{(E2od7-}q&UnE=I6R%+jH!UQrexImTU zDOHtPRU2VdH#mp1C9 zje2RLUfQUaHtMB~dTFCx+NhT{>i>SndTFEnCm+7FQ7>)OOB?mlM!mFAFKyII8}-sg zy|hs;ZPZH}_0mSYv{5f@)Jq%n(nh_sQUCvDqgGtnsQ;f{uwHW1OOATUQ7<{_|F~ei zzjV}P+{M;+>%_{gHKe)Eks~E7@n~IkWT5-+(4#=+}#}Q zjHFA|N=ZRnqHpNt2`(R_=eo+d?ayZqUu~}EV za^K>WK0g>-+2j|gF1U2h7CVMbyARqDkVV&@3eAa~6$rEm4$)QAaJ~8#+k;v8$+wm# zU@P@1XF=>0D^P9()C!B2wV$!t>uc+K<~R=OZC-boxe2ysZ`?&#S+2e+ zJj{6-^XSRJGnWF;TVf!VMuGm;eCLkxzbbPM9Z9?E@dOBE-yQzCR37sCs2-{TTkbCOP#FV z%Bg8jrYQ_{I@kc&LBD8Brz=mDm06!D+cR9zhmkj;keP>3VHybwqhhQUTWnKb$JVhr zY=0aetx${|R^p_&epJm(6RT*#fVct@Kg0jxjnVm`&ERz<)n;~e56#8rZQf740m3>) zCn#|(wJ-);v1^CM_i-nq&y{M!p{Uz)L{?HGh%pfOXu}$Q`|8gOwWjj^CNF?`ffkUH z-5sJq6I|XYA>q2(QQcY*&ByXx4*2n@%M)z1YV2kzp^JS$&nxu^G;vpW$iY8BZQve8qYCe_7}Os6dyPh| zxX7@$Q>kI3yPYP#moTgJL!8jO?p<2?`)B^%9kd1KlBvqmT5c{xs7_X#$}~8g1cNr! zbgMd6IQ|lfuxkL0sSr$lUKQb_Cd8Y)i!lh`zqq~ez4Nwi7veSn)lo|r;J1egDdr)p z7_3ZL&Ie*kVr@J~xMEe!heoTUAkK<8ZX6;1lbC7}#)2ixf#r*&^x&%T9B?gX%WVsP zvc&)%M3G!drgiwd7d_Q!oj!-5xgx(#Z31Nx^DH z=R)utoqbf+<6u`@d`QicE2|Qrd3Qxh=TYKgz2)*hyF6tLY-YouQB77-jpOaywC;D6 zaz`Um-aYtN16vyU!%76~Te$HC&pP40XXh-YyWaZZnK$>Eo?h?E5!OP@cqQmQVU9E4 z2j~^B_nslb{3dJ~S*B1G75c#bEa7FQgV25(jqN(^ih27fO=pdS^YFP&L#P*6TcAFm z@7u0={xWnQ=*&6|+9>wB(G|~%t^!xV=%KgVWNqL~&i=8TV7Vq&c{9d~k0F1bI}fC& zYYYvC0DjB-2rvz~Ay6Q{Lc7nFN&6)l?tVbMz3f;`iZze3JO4%FPvEL$KNA-zbV)K) zEpqa{mNF+~&bEvf0s4)fJ)zxtZq{60+o$siSVx`i*NHzgiC^HLfG_B#bpiB7nXT1TQ%ion=i2 zdFof(4ZiK+bbe}=D^)1#)-XvxInLr)h5YSDY3gu3d@|%l&3E3F)1G3y{VM-5LsgXC z8u%k!s(pj7fQjUN$9TrDR48yC=m_SMu=;ybWU^ zK9sxyV8>}Kk9r<-KYAVXQCJqK_xvuVp+zWD9HNXlLiSK$mVY`5=%FozY$q!!7zT`B z5A)i+-brDh5l!fg@vdi2U1Y(-!xSghd{eKe*HpC#H+UI6d>tR1e2B(Z&8uET@g*Qt ztROe#7UEsycN3zY`8jJTOFgZ-_ZWd^Aox{lV#s-XyhEVmoRT2Lq?(dR>2=V~qquuI z|GY=N(qW(surZ(&XoBa+#80Fv$)X3j!Gwv|;H5a`5i+*!PXJ$)-UGQrw@1$C4F>O~ zk|U|GBw75K!vtaBL~!Cu2jw>`h|O#?w_*&s8_jpK3y9`g9hCqDgvO?IbhHYMh5pJ1 zW9J0ehW$bE^8%O?`C`>3;Hkv#N}Ii6E=RwY4|>gTeOMBiY&(QR=(TzIx$&+h`4V09 z`OM~svY_pE+T_v&Zp*4!-@E;sHPt2*VFz#Icm~pO)!Nh0Awnvl0Kk=(vKLglCGhwH zk*c?pYIC*#QUZMzON(|<>{^*@-FF-(p(4~Cj}GJHHls#Y zKyDIzW%cX~i498ERnR2=x;!q%0)k*7;zUR}0$UUgy2+GF;3a+EjMAZl$V~9O6jXIx zKk6{9<4jNj0Zw^?Pm)65LfFkvOiqjnk>cvmqhKDR^6!Lp;u#d}e3fgCJAF)7X#Xoz{x&D=9A`%Abx%fkB- zcEWi@mrkTXv*#BiD2m1{li(*PU-qP`>&!h`@G~m&h&_Kuqp~ z)=HtZjNC07(e*51c^(A@uJ#>>hmxurqv%sY)qQyS;6%^P+f?vh36Onk= zSXs~Zo{V9HX;P3YWqE7~{x}U#&s+V7t!edqFR;e{vh*GTsA7GS+bwFPCA=rD7 za?G<$tSnd?zKMybZvdQxI3)(`9NkCO#!gg$Zc0)3Mm2h-Z$hSURCGYYRLGFTYe0&a z=b_I)(DPJFGM{6z2jj$B%_S@zdH`2_;z7vU)074$4Y^`^m zDCC))NM!;hD+Y)WmQ0U0_~2fIe{d#tPecu$c}Pc@ZFwRQ#Re$bVLu`j6M1_iE*VQdEdOfq_b z1vB;1YLcSvSHY}!us8IW@-(9nixN_gxu5f@--pf}j=nHVzqDA$=3hD^D`X36`k#Sh zb#=|kjV%VCR*BbQDw}bsol=s-tIgV}(zUZpqnvV&R>f^7I_zKPB73qtz={ucNVHoe z!TU5jiNv5)RJ5!R-eKf%Zb1w(qeHJ3!B&jm85a>4y-R7VEG=SyvJ@FXbFtQ|Fs1^P zL=2RV=(~tWq(TPyI87$9ih5D)IcR{_5JbB6MBH%gA|glvUWl&yMd$4aCUVMG&arLD z%&f&`HX-JdGtf?gHWk5os1P$MwiOUWcldNjz*Yp@MhCe!RA~Gl!joEWMax?!$aT4i zzZ^_l?Qaw@YL^iT$^u|0id;JtVD1IUQ=y(D=!Q#(peWnNbz$^CT}7?xf=V_DLst3GR^x~#`MHT0@)3@}dxe1hxWFrZG% z>$HV`qS=C$S`2KMS|231J$dAkESlZsAa_mNC{L+b+X9|A3!s=av##j-$W{A~oL`J) z3(uGOXgv#BjAnmqxgm>Y-+idHl871oczKi|Qu))&R@|*&_7q_(y*+ygkv8TmYi5tt zxv&pmsRPGk&1_BeOTo$D#b!3MX0e&gzw_YXjc=DkOhb{&Kh12x^$MSQSu>kGP~#-Q z>o(y3b2EFrPJ3`=l&qP(4sAQgzu{PW=ZkUce{NblkL}Loe)mSmJxlArdO^!M>2(^XLVc3Qo10v_+4`D%3?CKHbgL zt2o1heQkDat%7y%!F4xrgJSp~;by}_NUPiWp>4?182w#gUCW#wi+dlO7{$v2utBu# zS-$^lJEXkXbzS$Pozw??@{gA}D>V^Z-_6;?{0U~qqQRz@{1=1S0#{iuo85vkr97CY zslN~FQ4}3%S^%KDSqhnc>*&7xM4Gp(nEi3mIhbC)+zv36ptQ$uDTVdFTr=);;}zZ- zezkvAJ|3a|$=2SqwvHlaP>Y+9pq!KJ9C=SS3VXGyVC$(FCne73crPG4mkO1bF@J}0 z8ko6}*czeqL!k!lXmRU3z8!eVgn&0PQLTcm;(Hj zhFXWUt(xiR?DK!F47J%p+|Ikj?OFXx@Y1@HqvY0 zgUk(~Bd?IqrIh}wznx7k<2wnB*QOMl`J83uRo8Q$D|)(k>^zMq*~YX4Zd}e}dm*wG zhQS)oZsy6~ef6#x=f%~48@O5bF)>>6oz8Gh<-iFGd#g!&PI0{~mpw5& z?9Lx%{mEs=dUA(Oyvly$I#i3#kl?cb{dc*|^3lY@1PiLmsvJv7(9R<@y<}7|P&fdx zp-8NOD~4+%$$ZZ zzbmpb{sf8E&)kW;{%KSe%pQM>njvNESp9S|@{I~eKdbHyp+`ZW_Ig19u`O0Rhi75H zxit#F`jYH|C1L^%9Xbpu5D(UKQ7sp7R|F8NgKez%H`E1r*N1jQ%?;q%T9xQQ@+aM~ zn94>oGN^~vcBBf|_6cS!ft8ZbDO#SbjESAcjG5Emm6Q0B?v{Rp_f+|AeJa>!=VPl{ z5KY*9DZh`N?RV}9{sQAez$g4sXQJK|a;?^~r!8H-A6*~l7H31jEL*-BZ({d6VNyWJ zK~uB+#K_5>Ao>gOL7(*mt_JWreZ$IW9C1cIyCkz(%SUX!3VUVs=ZW6Y^>YPt)VnV~ z6FB}x@8uCrxsw?q&E}r))d#-Zj~yDPDV9CLZzWY+^dqhwnJ{j+I2f{ewEgY}-3sK6 zaitn*bO)NvCAK6t&oiPG%;nd!mIDx|fn;s(f~AfIUVqjD{>%8qviss&k6qmfTr*P( zGH=z#dBVw)J-sDun=QXlOozv!zuDz@Md5nFs&HPnJUIleHEFV+A#dhL_e9O7s7>dkm*(HXekn!{OsA!9@K%{5GizB z@zYr`ShvK*u;%TNaLFvQjB+Zi#z*;ptryx?SDPe<+A!YeGw`?_mlo6FQ9!x@m zui(drXHNoP0yU>uh5KaQxfLLcPruI`d!p^cfv=z2*(V-xP5m~vY4AF1#63&Z{8)S= z-isIM{6n*npR0{NF3+Fv?|HK8j%Fr2pk`#3e;V$!xbBY`+w;|_FAabYb zQJ|;B6*Hgx3A5|zhr700e;gRTDER<74sCg_vs@ASxO6s{DIO)8K3O}E`gmE!i&PP0 zPB?2U?Oh2RFaJ@}99&SQaAm7kM{~yA*XN)?jmYOepa&i+m5!<9{fv7m*HZo~^kGdG z-Q~E_+?rv@`t}$26rYfDl%4Q}H*G!&`r>Dy)iwm1VC;3#k8`J2w544quL-i+?gH5= ziGCDSb0_|i&KhoubywsiGB(F)??~GXsK?PKf89kxjkma7yDaypN;)f=5A< z0r6*@&vVT?^`*XtTpYimGxchR2gFe>==TGA`7Y9cJ=C-?fskF}W#J2gt2bsS9M6_K zg7w5#OWn(>3(|?l&H8xl$8CT~u*MHV9p{r|vW|_yIu{W;P^{1l zBq@>(wbuTK4zoeiNDJj1Q>Q=dUZqwVlOpF*jQNzVW^#_Kf9R##b&{ zMAt;beO%{+Wb7NkAv1*Vv$`2-d*qdOSZ&c>m?P@+rxsLC$+FqZ=K}m=$%9L}qFJ3s zUf-W}!plxSybnJ}A9C}MWwS>D<24f{9JLZMXaN33aDu&|y@3iLH=6Em(=|JisjQN~ zJ;?XKCMw?pj!H#3+5r?YTfWw`-b;NyXy&frk@`@--x*zNiz*K>xAgX|X|%A$b@WH1 zmaKy=ND-^uQhRu66TW=rnxNDhhXy*gE*)T*?zaJ#+`pVAB9|8@i_<+z~BcW+> zZlSxks6=V*Atn>JGi7FDz;xEmj@HXNTkg*>ifa>|Sw(gKDy)Y=0vw9V6?D1n8@u#Y zy>GYL(tY&%vG~xyvWDNPqcN!g!nWxW!x_q8SR+D5{%*4i^eW!jCePaOG@qRG5q3^vi$ z)jT97C4Dqxbh9HS$`7F?I^T|7DEs+VKQ8rV*#@-Ke7@%d7^nw8pI_XBY(^eEry2s$ z$y>;-cH{4hrsl$CrR2ADv!RvkB;Ljz{9DKW+PsjnyJ>_KU6_Vty@`Mx10=S4i zWa*ml5ku9{E(EMvCogiUV?_uRvN_C6u1cJbm<~N+iW=pRq%ASMPqYoaMn@JQCzPz23`HjgQdLfjRsF=~ahLbzA~XT#JI*FSy(dvIFXv)eAH2fDtKp zPrNWgm-!@od=C^+8_sf*{q|PNeP@+in7sQ&2SJObuh*5Eq1hck)|>!Vx|uW8Hh$H3 zy^jN`PcR7}jT&eM0;IwMsA`RB~O>w&XG=oS;k|OLPzbhpPfS|+QC6MstmT`{41#Rnz`+wTmw$lvIVmmumSSo90 z7wwAdD6-(hj#(L{Ox*uhJ6m=pD*4?jdF1h~rpEh+CVw6@dTu8X0VeV8>#1V=p_!eK zr$DqfJ=dIQ_|#YudayY zx0TG9BOZK?Hyw8EDA{CwNQ?uS)$F>kUsdryRgku|)KlA0f;R27cwVx*9vVXJ zHB;eWW_o4m>~c>>-&xbx0#1xgPg`*>iOdxEZ;0&Vx=m8mAMXAk7bk``OX=Pl$!<2k z1Wa}E%_Fb8+|Q)XvWvAaIX927x5?7kXZGw&-p3O9u%lVG9r>vvHeTqwP~@3C&fT-N zo|6-|JUb84gAmppl9U)QJA|ccq%`BK0IgZ&PiH@wID|rkb@5ETlQdV;+1u6If}XUv zDXy7{hRy4Mv2?1+*fTTN)88}not(>LUs;{_8XxnSZ2`j{nGi1r{)<3rMIyxXO2FvFBmeVF9AOqW2JJ$<}jE z)kpD&i49S~G>~%9dqbF$0kACa^r--f!@w;IYYhewpCRQO%WoSz7}3?*et@Ob_`7^^ z;wDUq?&}tNa55{(Yu}TfU_*M+^@Ll7lrPGJ{}yQ2jIYF+a4!0`J9xam1 z)@*7ml_&-J7OhVy*2;k zVlrsj$}0kQOt2k#{*#sbiiRGWS78QYov&lQUe?u3_gY@4ZZhY-Y8d4UZ1J$yv!mr9 za7Mx>aZO_d8yr-`w~M6|);Ib$HJrw=(nx$GJ|UO?Z$U$@&9#FU&y; z?K~Yg*>w4psJh@6<Qr` zfJ{kh%r#FM2kiQp?0$3V>ygU}1K^<#=C}o0NZ6r{Q;K>j?pb|$CH5MVh<0zRA)%jB z{P~2M-YM`^FRg~^t_#QL{QF*DBIm)}?)LfsWzC{7bE^vu$@k?&CDe6R~-lcRq@@ zF^IbEK7fc#%P(J3CdzuN*@)pN=xiXQjaaQPyKuT>1MkGZpnt^)rHFh9-qqn&WM5ob ztaOzDqLhe+DQbwl%`Xrv;K=kLvwDR?Mgw-}{-bjDUK>ruz{l(jYDKvb5(ZbNRt4FC zo$Hu-1pIweB!mK3iM+0PvMu+Zqasx?ufwy!Ht-Mn1r>Ve{z1RA?SA>=4MgjSb@!+s z*lsCj8-*|b)2kf4r69*(#+m)XI{+mYxE#^sqTr(ff_U&b+j^-)zddI^FZ4;T@A~8l znlHfm$nR!7(edz!E=aLb5w;oC)8s>DJOr_a`FBsOkV}pSn=NOy?H5SZuwCw?jnQZO z14V>F%MvGmcT#~qc35~M?0a}Q)Oa>JML~?Pa`R*z*IACd5ZsHf*Gb>MF$|sK7P`z> z1bB8=954_orcdUbys7afa&M-VlN-9CKy8ABa*I#_6Q-(-OzHrrT3dqF&+G4>F#AK=nMEM9{<2@i&MeJEL=BD z=cXv!NL~FU2$gh^hPum_s6Hr9k&6YQ5`9qj=KiBbeIv--YGPFVk&2t~hfr|DTr~79 zw;lV`1r%e*OjJICMvscqgTY_bRvhRw;fn&!1pBL1O47i!(p4c6deX{Zlt(1!&ddmj zdsl_!S`v!F54oy#dBK&z{#iRH%qrvB4(0RyU6H+t<}VzBlUtu>5Csgh#U7@f&)IP8YQ$CG6^p`4ubl1`Bp&Vvm4+%U@OQBz?=D?-xJnr`Uj+9Z`ef~mx+~u%k ziXxb3+YeB&mfzve7`M#V$-5QdJB>OQ&IMGFy_hVf=_kL_WJe^c)w zilwlqR2|gOu9)1DloKB>GQbyTMsnn6cw%|K9%f&+PGD&Bp$<^9+%=h97D&wA1VI+< zGPD>^cXR$@m%WDnWtWBI+sw_X)ZmsY9!;?)#aa55%G%buAT|OJ=Eu1zprb_X7+nOf zSXTMOaO|CS9usCZ-j8Q9l9xTV&%kqCFsB=e(M7&be9}m$%J^$%ww6F00_}&y@YhOO ztX1|G_4mzJrxyM#vT-k_HB*rF^(zj_kTQ5^@AgYKaUc+0MF_9~j?Ki0&8x6f+`EN? z)=Fdk_}Nl($su18FON24&afg_WlK5AFI2Kwxe${dXBYSKBHCFBU#ZL$3bt@NH+SiQ z4bL1TwUQvlA&`N01Bh1_8|2@re&ybK{$b6!TogpH`C(w6t9cvqq0vAhT(uWKDFPhJ zA!j+g!ggg&X`2yk1ULIQ5;A6vPJ6c#9L0SQ$KyJlnr7AoJ9Xe5fX2Vo6I7NXq1d9_ z2t=z6%93()&3RG|Mt|GMbGl3?sbsP}O&2vQfE+cYV}fX*N})`F;jv7=9biiXvt~f{ z7vxp`l7VZ-u>aj47el@-o42KsiwkhaH*UXQE&weon>J^H2w*rx#SJ+Sa(5dSA=fu? zPbZ_^ImEw0qsG=vZ4CqP6$g)>2Ot}%u&-1YMS^%pPcqg=&C@Y+%;2^4=sO*UM>fMg z@{wzKP-`*pS^}>)jI$viY}gPIn`p-Z%~)7(6_fO-pdf?9vBp*Ld04dTjeex^(B>3G-@0WCsoN%~>0HdBlyeaTueRZ2B z#TZR#_9IqUGJtqCF-rnuFWm4~5@05H2)7#t|BaCe)%vf-y{hvOp?t@AF~W%sY!j+C z(YE~}faVE+199IV3(_RP%Q0XVn8ykQnG`CbUI=z4rXLh1Zt4Kw1k9Wy10lpq6j*^2 z#6pux0t2J56V!7C)yzXZCE*GgFqNLHp(!gnBE-Z(-If6=qreOK_{qNP&((N}7)b%WP9I&yY9W6Kmb4$w0r^;>8^v|`ezEbln1VWJR@%g0J{s(?Uk^=_vl(0lte|oxtC3m zoKO-67=`bz65=iTbIhBwEY}`J973S~cM_Qpb-Eq$LV_=5q>SEBP#5@rrT~3>99y+8 z^#$Htf-?9CmzUtLQ+K>1z^VucKOvIH%aY#Jx+b&8B;C$r*qeV_WExXuk)_KEr*;E@ z0(_C|-4e>p0t#=WpAAHTqJW(L#*r)6g60Kuy--lCp^LVd)Tsy3qB~gf3N;pZ(Dgzz z%dU88GrB=WlGzlPI4$Al{)zOn0{|$N5Is*tU!-jarX%A8Idi)+Zcv#g$n}e@>zKqCQkjEV)W2c!>2Lpn$%Wr5{^gQ+ zN&n%JNyTetR}Ik=VSJKGG(fE*Z+yU&x#S4@0+J9s&w!j(z^>eC|{6tHoBQrmVRF*0Mj1$$(?`zG`G4X%{Mhf{S zIsMl@I~h-2S#CKmIB#OUZJvR!S27_>GX5o$X&3$x%BN*QnPN$%WfsVUG9l_8p&V;} z>9+t!V-n-Of#!u!V;NGG8qXa94u^uy05A_K_HjcclMa$2T&*UAhgcR}G%8p}MVAy+ zS@8gkoft)mNuipOVW?-F0B<@$QKVIQ*ke(2=pf;$;=nm3-7b{@suS2ECa&K3Js62I{y$X6YhTrtUkPjK=UOg|>@S+4a&Ns?1Ns#T!Wi zsz@SpKl4>{@|WYCuMSfi7O66`;k1k@^XjHvEK+5SH@gaDRGD^Rkt)*~+G$lu%*M#BwSrphiiE6@Fr%GPHWrShkX2sEUDOFXHi zW@?oy!4->GNIme>x}{4&7cL+ttaA}mX{W(^3Eg8zkoAiEsCgn zj0eURWWvcss!UH*jRrv}SQlzT5CLZ;0e1*7%(GQW?pQM_q=$Oz4Mon4c_)bht@45J z`k|?Yt-l2I%X$>5T$YzKs3-}MG#>DVf`tNQN+ggQ1L{J#l?VtANWf+?eJy~rGTLps zaSnW$T*g`Pg}sBgHza5{Np5C8Xq0vbCB_}!Z|AON(va&sDI{vdsKnj0s&NJG2Dkj7 z%6FbcgOaGg8DX0#Rwk8Of&`tB+Ztq2+0{`dm0RV3y7m_Eo?-w+t7ET4Tl}>7=x}9G zDl@NWJ8HQHV|o^Zxcv`>GOFy+s!RJr6=eb7HtnO66QD&O-N62=tYmo{s){X><7cl3N)WK?-<-$Z2wo6Q=YSoD&L!3H7F3a*4~S9?D)@AS*r15MErP>DvN~)-47S3GEK%~^|8{e4jQjVt)x@=@)N})4WtL?D8>VB4BTD; zdL>>=r5j-ms9&3T)IliBxru98@VTX#^i27{?H8D*@FB2D`M;=gVbP;65a`rLo5n~G zes$SuUx){--e$O`_vS+nLVJfSDB)YKDF)w9ci<+I?kE$VuVt+LWK)=k3ZkE%*y|rm z#Tt>?2L+2+Y75_XN4o#;uT|T(BbDd$!otMM{9mkk{?zG@R^Iw+)mgzy9i#Gz{_gFo z;rdCYVcUtjLNWA+zH0fCPsF$>H2CtzKAFu8VV26M^1>ojemYNApE%U7NVHbz7sw@p zw{(-Q%s>*weln>XX<8U8tX8}TgNVVU%(JiPWi{%B_@B?_7{R5w*BgeJ(vN~e8?1^{ zFmI?21{Rq3Qo>Mj;lM`mP{&Uw-VT))gq~OpwDLZQJt%g6^Of!f8#xIEV={ZIP(*i^}uAfK7DHS zFaB$*4VjBhneH1aJj0=$+V&omDar2A9CGC2AktHxCSa)&bcF;(d^LLG{=h69E84H_ zM1)=oB(|rhHy=>1vT${GElGd0`?w`w6O#AI3xR$G=12?vk;*IQvo-DVh_57qZ@lvo z{JG!_H!NP@qcz_W-)0;heW7dLIElxXw8*6Lo>y<(`EMs>Qh9+68T~tZTlPEbnzHSe z@fpk%rvNQA6QXi7g48Y+joN~ft#wt_XXSOF=yjTONOA~-p7|m9bM}w)ccv5|GUNt+ zZU5EPT7J>*iJ_iFk1Z=6dvl4`ED7jx9u7_G%gS?h<0H&9qc{>C6$Mt9UzJLGbNLG1 zoxZ1(iN~Q{RSv(nmQ8ees?s7SL%Q|I@IfbeL~6Kp4%5Oa-(E(Q1N|BuDf#C}uQZt` z$5*)p%!UC8+JW-%Tz7WUyq@P+rJ~5(K=?GXZsmnGvq9puEjvw%WKx;>AmSp<`SZV| zGPJC!l`b7UXndz?hoZT>osFd*FqI|WtG_@68=mZKlu>1%NB4;2_3f7iS7lWB>$6Yz zq`I#fDU<#>*M^41f8TswwfYiCm>qu+gxv8NFOoyI3y{6iVom1KN(Y~wUxhVU^CSDwhbLJ69QD!WF9cs_qY0T-W=&^KiOze% zZ=Lvic>kD8D{DN)Z{^h(IT2U)NR3X`2d`f}ruT3zuO?&17v}w$hzICTT;h$BqInYh z>Lh->|MCSe)PU~y45Cw7HaSm)A8{-={&RY@Q<=Lk`--LonYk+RH9?&|yfOR1rVYOY zC&u68I=9^EJGZ(_MXPXtHx%KGoLmTmxRl{mj66^fDf*3w2w{`=Xy6weDcPnY_}nEMUp9X&;9-<8!~lw#mGr&o=8|MZLAVGSebQ) z^o#BGduGI+fDUl*elA(h)lxQD?a!h%83dM^I{qa-bP^GS%$y$icyaS`&C)1rK(|ff zL4VAAlh@eCo-Y?^is9eRmXibv8!=g(I=fNj?wO$9M*E4Il2=T0oupC$XRbG<)M)j? zvA?n-s(K)}QrmU0rFZuIYHE!8?X{N|nUFJl`ons#;%cM7&*It~Z`{gE{x^>J#hv4l z^Jh_FP2{F|u(4!gChyDpKlpZ}<1Xv2!^h1%d2S)sa++V{n&u8}czXMrwH zJO4W}KC=w>yF62r0!UZnCq3qg^;Vb-u{pLyZEa;f@?C9pry3K&I=URw@AGD9#o=ko zg4)iTy4PEFfz^};07DF7XU44@azSV+sj|X|1)b()8yuhtMy@MGs_Zu~EiE|d=a5@l zukE7S4lz6?pLO9ON6TyBk&CdRXpiT4nn1#imZyxaE8G?egKpk7_2ASx*;7zlztFqa zsb|iP1WpF1)7tL8n|GFduQ)X7@3lH3+0|x{^8Mxe**7)3>rF?j;TV&6!Ha4|q4moQ z+o^G`je_wdOZt{7_6j2{^nLBmEknXi45RM?)Z*b7GI%JgX4>^yhH#^)>APjLz$8EO zu0XZB6C)S6TFrXf|3WR1!??a))$vngU0EDZTI*ZEfaa^fx0esa-4fvNG{0* zl>#Ms+Z47Jy!p0}m9}EKt{j=ZWmI%_KA7 z%pg(`qVkVcp4@%?8gJjvx%EdE8bqVoFrseh9Q~Z8FK7%&Q%qI1$l5yXLn)hK zhji?_Sa6B1I~rS)kyu@(8`0%;%{Fg^k7BvLY5?$K``1K8`Y9NaDl%r*dmrufJl(R# zyP`_Zn#kO&QMHS;rlU6NMc&(tR*Cl3)A%!gg}TV;9tM=YzR`W^@)|Wm>x3@2RTdbJ zf@~_P@Ik6w6W#gK2n zcBDPd&qUA(b2oHMJX|WRQPNTYYcJRhnl2>taOgs9Cuyha!+O;Ezze23#~Yrh?tbufCKu_hp^7h zR57zN)110k){0Xc*j^t7Nn6@FUiStxPsu+1ijQ-Zuo|;5Os5-Sy#K7HR!m@Y+Ky{{ zovQZciEo}d^WUJRKpXfQqryol&iQ=JfH1#6sy}gadZc*Gs-PHQGAK}*vB2bOk4f*r zi)YvvsNVh1gcPR`CIFr0M^7>ixoT0MPlBo7tML%*n7QNfYXJBK(le>7TTUBF6@P|t z$f<|r{;tYtHsm7CmdpX+2huUH$Bc8I^fKg*Duco%QhiV>b^i z3RAS~hWNRbZBR`X&5dUK8@^NGtVDjm>A;4j_0AOPs3nbqH`23@`6}Jlt&dF&rR|n3 z?<fLS5?e!YT&-F>s$zb%qw%R&;K$NOEadvyxNI7Kh9A2}U zUuT!!_cZK1R`+N?FK0riJxa$cXkw2sIO=mwnSIG?en=Umjz{q}dYOnh^N}{W0HIfQ z0LeQeEoy$ezH8)u-{wQfy^4Z1OxJ@A+p|p%Ox%RePb4F@`4>KH9U7GPy2rRlCzv^I zR(BA3xKB~PCa!NYKWn>t>pr18l@sp#bf=qc93NL3k%VyA2?J{v)qn+r*@rDye3eHCT?LrC%axa2C5YAPjNY2tEDj0sVyr1=^C zc{FHeaf>NwMvWY>lcjA{Uin+XB2#*#?S&l317Gf)L|(QQ1tdc6oyuINiw2x;f)@xN zIE93zHYFUGkH6bgR!!(%e$;2k=`fzm>n)VC<#%Fd^I)`xKUD^)8uQ$1pH;!WEBlyo z1yVO+OIz~Bd@C4gotI;H1TXODDB`1bzS9GK6QXe#T+@|-_znVVs6C%WO{GC}A10X6 zKxPzU{zzj5l8L^NnY5O*{AqcWi?*B~OE;c^?PG}ksFd!%bLPBWL42TM-OaMpGQK>^w!IcmsB1*sl zDkU@pMGS%{)&vky5rd*4iiV=1%N4%aA{r7lH{71az{?Ep1Xi6EHQii6Kp(*8m#cOCv8Jbdtrj(&6WoSwnno|D9 zRc#40r3_6eLsQDolrl7>3{5FRQ_8hn&KA&=GBl+OO({cD%FvYZ<1#B~N*S6`hNhIE zDP?F%8Jbdtrj-9rPANl;`agiR-v4`Gt@8f_ti1!$u3pgB7j9$8jtIkOU4f6k4fiIe zOn)orR1FPPLqpZjP&G7E4GmR8L)Fkw zH8fNW4OK%!)&FIv8vj2!R9zZTj6-7JN%pUa_kna!j@gkGAz~)7%Bx zx_8mkUF)JUt9Wimqa~kwA|p3+Tu!yFqGx`T?Y*R#B=7`-WY^LICxbX~^AM`>Gr^LB z^g3YRO1f(Rd#Jo|)Ppe9m;S`Gn!KAJl0Hc=F1?>03hI+?)FeMnZLFaciKSYquzQHAg-H!IQ>sT+2n&2^*& zF!QYnaHJsVQOj+A&;B5_2&FaVX5?Cq;$@^Jws9p&VV;^TW_~z$xA=E2%kjN=;?WJl zJA-k3I`rP=S~v1U2;h}shFw*8zMM8vRoA?u&(hI;c6rqn^90}Lk>;UbZ_SK$^ML7a z`LWk4>NTCmO@EV>Pn6Arr#*E=R!J7r&Z{uDN~0qOD;u6&+TmUK>$|qj0B74@OKW8j zei3v$v;Xbo`6v8JBiDjCAv)Tt5-ZNXUhLF%N{Lnx2x>Z#a_fEaUy?Q7-F~{TIdH3< zgNk$Fi|5{VJTIlhXy!SviE$L+a?GOx}{Q>iqqoPzP^&&PRC z7JNNUJfLTeVpiBau6w6qw4&}@&$PfDE~d|u?)yfR{674o=*ocR79-$%pm6HrN1d*9&v}=b zj>dMVc>RdRq2H6om!h83T{ST8kBRrf%^;Ntkxvco!xV?iQoZQvoH*u!LPV+0J5%SN z(TdVa5pgUIsBd-MVo_4i!o&`<-vKrp}TDAql}35};SGJm8ocH*xbDezUg*}o*|8ck%YJ?n5@ z3>*mi&H>%(?`vuC-Z9qau<hll2VkD$1m_*-iqPjs&O|F{ z#LhNcm*J+Cj6?3*0>#XEX^w`f7p((M`rcXLqusIUbsldMJzLXB&b#u~+T7qHrhqQ< zV{=*;H$RKi-w0~ri~V2^D@`dJdFO_R?dsDUPy@J0(61tUEszPVpM7=Juah6MDDKc~ z9PDc2sBWfAX?~HXwFJD%-aGyv$hVy{%PM(adBo7eor*NhP1gPd-%CRwn`^stTq<0? zOJypqfWTS`r$4OPZ|*;o z2uB((WILtazDA6U*UrKmmkT@uQt?|k(<+LGN}GnO9#e9|t48zSb%Hh5?1Jca{bU$J zaXORJ?l&&GZ+zubHG#v%et5qMmP;j3!cmLSDSOy4vcuooP}+?0kU&Y2;iiXD_%xTl zOCGYDUvcUVvw&1qCButu$TUyj2FgN2GKjQSiw&Ue{B*KOuCjf~nX2a#aop+`;MfU}F9fW)m*_jC)LV{X3Z zv!PCh%_?q$ zU-NQ1@%~pfQ;I+{KpId^w0lsYdhKqu9$Ji^h`3n$SyX?-TTXUb5>#fPgEMtS-!)-6 zHA;&>T%hqI&$7dWFRRWU;q~m@T;oo;+;RtH!pK>unABB?$#OAKyeUMt671v&nezdD zvf;eghhewk1CV#4a%sIMeg!S;B0+hC7GWpdOB*bIy9+SIFOu@z?3r*BFsPCuYV&!e zL`80D#)qV`j~_n8LvM&uA9MYvS8>ij$8Ll9Mg`E(;tS`!PDWK%6{VffCR(TWV9^bZg()4NgMkakL6P2#Q z1q2{W1xIe?4T=J4g+h-2R=*_!G242v;s^$GM4olW#w(x_DVc7YIvW!0$B#SW9q2w7xTPBxx^ZyPsnGKJ(MDVSYFDX)#*~8Q;TFezDnle z<3)%PeE?R_} zq6qnp!YKf;Ux_z32;a^?HKq`k&oVHfQZiLNU7E;FP>_F7>~`z`NUhi`X(GQ5UeO1i zVr0*7y~Y#a%X>o0F~}h<0BQ?EM7UVazE}}1gqg!jB^{;UJbCE_n_`2w7+ngipav$W z(k|yZJ!QdXfUTTY2^T=B6_69cK_CDag@%8Ig$X$7331xKH7+>BA|wrVMM$P`kaZlF zaOHu2h-FR+4~s~Qfz9LY`RawA7UdqSNmgsMQBA}UJ`z8Okx~w>TY{cpV4}t35C)DS zC%Y(EE?nY_6nE{bY9tkqjgSx^v7hS^xf)q2-WN-Q&BPJz|4s~I;^P&_Ty8E^nbx|cNJ<6h{^9HFFKJ-r4TfwxRPLa=P>X?s*(xj?iS%FU}5|rG+XeM2s%>8i^CA>g=9o)w#Pq!YV`P9apta7LgDK~Aobn^=LkW+d9M zMUBkV)~1r4C>$E%^!Z1Z*Nd_HxUe!Vws2Upgb&x^!Y<94i{cez5X7z%C(qEZ)lAZi z6i(wDKG06|j9ld@q+$T|TzILr4C|#J*5lw-3i6V;r81i6JTLdb=7WiGQ3ed`LKv2> zAn)cND>ff4WcmbFsdeih*D4+8Qk+f$d|O<3R6F8DABMjfO9GEK^KwzG5hfa7qH?Mjy@Bvw-aB!w6EJf?RzR!%)Y zR&guhJrFp#Z8H%+N(*s7u&aXzkAEGW~MKO-_}t@mNYwgZtc*6#@2Qxbu#sOB7_k7B`t3wuFhV zqG8)Dy{c#BFdHuN!ZFnRL2}$F48intHZRwZ8my(nZsNE*m{u%c0+mz(QjF<4h1tPG zJQWiSKx`HBlccK)8c@%$-Fm;yc<-|(F)iV3F*$`?-a9qC}MH-K6|Nqox! z+Gxn;W2hb`!iK4G1(3wI`?V|J_7q}>GijKKOnr*+W}dU>6a+}}-7k~Rgj1IVS#BCy zniC1j1yDE!sf`JMn_;RF_WoY@3eIuj_A zT&g3W2AJoDtTzERjZ9Ah}TCcWyvJE524(ceC)=={4>6Sg>_(BHCFE$xiKL##!#$T0f(_(JpZ#UnV;FoiEvS zr*LcYYQpWTr*Q2CyLbwo#etg~yYUu*^j&b_fXXfB_RIEMc*AAlQ#tx6<#OX4#1)Sf&(>j& z5X^p!>eOI$%X@F{A0p3%-=}!T*l) zTRv|oV6DIzjS#brp8L+ux!y%qduj2(=|TDECcU?gPS$k|43c>raZ-U}r7v|6-4BuW z8dafIFmnznSkJ{LS_hK(ljwTUvTz3Qdp0gW#j$F#y`wMap%!xS8tQt{RbkJv?~=z3l>!eD z${F-YqU8%D2R@GB_A!Ls2E8tv`(Nn72rjmshA(A2Is1cUb%xwQX*&O2jU^Un*j8Q0 zf2i*5?iGCmQ?d?R1YA6`kMD!YogIJ+m<9b}VjTl$2oRnV?mPIS?;JblguDjufVY@* zQ{xR@84u)D_V2fIvY#O56`$+pAjSp;JjDaNKKMQ^BFS6$Z2$8EciGSC$TUU)k+#X*j~#7)w2 zepz;ZV(YLr%q+KIn9`#r(S0$ihFObDPw8(>MiMKVZ0N8aPV3GpV(VL!g924b!)F-3>M?t#GSdh*G**THYeyI54y z_t@oz8uu@C{iyGve6t2h-%T{bwjpz+kxh-E`N)fK#X z00L>QSl&Ck$XK6jHsyu>zTlC@E&0`MwkJhsV{JzlEqOCTIe4(Ax1@FTqK^#gfsy!+ zOY?kUi9JzOM4zkP0*cUqHt{4*)y9P?7*sEDHula?F=sB{pMY^#VTifjURX&KaK=RL=6vIjXZ_Z`{Y#G~coUA16%;4gX~4L|c5vG?jx4nVI95*{mgV$=)c zUp#E{I@j|L%E@w=^;w5_B^H_a;K8>Z+_wA(B{HD+{*)YUQsQn=`*P<7#S~b^*iY46 zK#lxkQ|w1r9`I(V_32JJatH9Cl!>AMuj+OyO69PW?fH>t*oFP%L$j>Ww{xGkL388F!Nfph4*GyR{+ZB zr}2Fw_l(>G=oQn^eeyx}o{!V3o?bu=dgkibW~4TeeMDPXUkob6`9}ENs^YPro_oJu zH`s>`t{wa}XdiEiHZpRn@djAP$|9li8}qJ*Vxx6-0X?;B!pwoUgkSH!zTvp`%eH}2 zir28+$V=nK^VE^urBmUce28v+Z+ZWoZjGErdt``d>C^&c??PaA`InNG@S-xc`n5jo zEjc${R3pQhun)f=w{>flj_4MAOTJIKRQ^5kPHiXGeYfVc>wscK+aq$8Uqm${8a+GQ z>ZfKVe-K%-n6fxr<$~->^^y6l*%#=pVfO3Y5o;CkcVla>r=B);6<%`a+lIOO0 zu=OI+YuCNsZ{U#&*09fC>_@@{HIw(olbPSqvB*q7{?7Q_bW60E(m%o7+m^1ztmDyB%J$Gr@Rxs76cAi~Sm-*=efrlRZT>?@6>~@S%jtoy(6^CV*>tdtICC9Ej~tHti`1K+Y;L%RKhVsOiQ2oquE*fF~Egt&B_z66Cb>Hx_YS{7qx! z3AnM2mqRDZzP&U{-gCKZC0=Pe)3YBAoC9F*9$iMP#_p=ti9i?^%pR!m5O0ZR=A)*R z^p_3vOpOol75>BI>NJkCe^eVdT{J0^epDC6R$O%@Cbj zjaGvN9t3GcqlVr^xuhHsbDmnH5T&#&I@1MP3N(N!;G_jfbSH_i4n5A1kYo<$pu$0dV#D;l`l3k>;Mh;`Q^qL~EE{>n#U`mYERKB#dGn#bl*RAcx z>;y?8_Lf;=x73;dETN0NV@8_!>bWX*cV~0ct%Qkh+bth1RmcFVRL>PmIXPi68gYN8 z48^C~k6&puTF3<8JCrub6OSy&+N~^W8gI=N82Ure>099d>^FgFo#PI`c^3tkjMAV~ z6uns*Z1V7AHO=tJ!*>XmL(6SCH5!D~e7g2)GE&HLp1=a!x+>lZQR^{-8ncOdnn9Wr zK(A5oAM6mX54Cfrp75B^%jIFhs?615?F0rD=bmQ+=L!iN-qI=Ain}GhN@IDvq=6fK zy1rZ$L^IDyJuRz+)P8>9tnE!&g$Zg!1WjWP9nQ>djJ6s@>LmlGLtkcPlN`^^PZGD5 zOxt2^ze}|qV7Hfq+@csvvh_b!vP<6X8g=@*fx1(E({S2_;`RqG_~3ic4f$REv1L`F zj36+1&uK3O4v}1NkNmb%4mVMgVK%kDp8QQmeOpzSk%Q9P$VGv-?zMYZvhf@;g4t`M zEg($xc910H-Y))A)=5Qz#Kk?W#l18-C<$7*wL|DJ!PL96@e3(gj%-nKeM9ITi@!@i zJ^JOH&wV@#_fGMP4GDRdGx+ON7{bDs=*%rVsUJU{cg;n-XK=9(z914?7{ltCa`c`U zyXxJsog0R6@KR(Y!ub(A6HCoWmKw5lNf4KRZ!7_aiRu(teWn>K_;;&3$Ec6YIY@1hxhbya5qD&f-U*%!(Zd>-zS=Jn{v7 zlX;)`#4!-JLfv@1i%*uxN~PDH_RJ{s-W&5Xl)t~8tW-IHx_}CnZI+EAkR@xY@9Pbb zF=H!Z!#OamW8)SmH*-KE^vL00hI9W_I|uzZDmh0f$T-ycbo-{xD{b3&hD|@pC&n(L zG`apBN!urKV|}*V`v$k*re8?AYQgwJivk#bL*k(53q@Zj`a;ncioQ_vg`zJMeWBl=0ya8~y2}xf_`a;t8|G4zM?)8oQza@P|vpzC5rQd7uYKGud z;3ydw5%BAu@|AHhS4}=pbSoCOMBZvMttPyvR-D zu{T1OuiD)5D}D(zTSuG)9}Q-H9>WR(EmrQXRAj^J0LKUgEu9gL^V&Li?du>n>#_f! zeNz!J#&A!8Pp16Bgs6Ms1`PErENUw0(5$L$2xrqY$8*`T&V3tQ|AY6cYmdFumF8K$ zTNGlE4?q7!BWUmIvxw`H0Lh1TSWmEkXh~=3pBN01R(tN4Nric_S{M9sl&-#i>-ftU zD#no+qtU1ROovpCTJyTeh|jlxZ=A|cnlvM>7Fv>;IRQLFjkrfb_5%Ag*Vq%L#4Fq^ z^4Z&Y+p1%0EqP(Mbu?!U2@ihv;_H_r2}$dag7QP&y8e5lknG5-!%Jc2-JFGmJY-Jp zQse@gH3uAPSoPq_EHw{U_Kjpfvt|sln8e6A?S7EhCq-NJ78aC$s|;D&Nw(^XQ^|;d zpZlSG1LIMi^<%b{dAjuTb4|niCG$_28Gf^SFOJ z*v*f14eQC>0AWTB;M4)ZqGx|zIG%~=L1NX*VIvB*gT7=KK`Tdd=LlL{&prf!NhBP& zoxGt_ba>s(-$}0R#M`jP*(F(yCT<5k!A^hVkS?YDQk{a1dM(IQ6pBnj~JHZr{9jp7|TN-g-yYi{Im6 zhE(ibSri4^#n@xTumxG4Dztk<{<<_d!8t>RqCG+)>-jjD%dRI7beCNIW+fD zWi)rr-d!VXxr#PSHx9PEA+P0n;Z70@5edn zYsFurlDq`@#X{OI-$f<@A9>K-jutJ&Mu1T-1JtgxwjG(Rfjelr@~Gft60X_Ht+$*P z7^K*9u|cZ)n!(4diyk~&1nRX?;A&#lAYsK0*YNs)d9RrBTkQ=(ochSHPA)O@7lfl6 zmb(iWezW|npil4JAI6U(Wr8Zu`kd$d`IEzsd{TCx;PVA`0quze}xd>oQ88|4q3wSq!6cck42Ja;W$djRs~4@;XK8;W_mxLaK%SL%llvQ@u(~ z{zJWvig1$v+*69bQ8|a}@pT*|jfo8rWiZ6)nhaFDH+~xAe|@~=Z}sYX;Xl0V+iJH* zFUiHC_~$3pVKo1D44_X;^bA?+sle$?qE)54mwW#yT{(Oz7q)YDHSKTd`gOzB!xF-b z5;6J;SI1fFvH=K{kdKV6^^FYARb=lR5m1!qiLbalMdSw*%qfaWB8RUoirTqYRg=5- z-a&}Ks2s<|(IHL(` z;KGXK7UrM}Nlxx!VCPR_jsmc*7|+e39Vcqk{298sz)tae{ArK%D@4c&7K{jD!8TOe zSFcN{{ElpR15LMB3F~YGet>xER(MgIA2%-h4f|ix^>NX^q^p`|T5~*16Fkr{tA&{X zutF*7%qudVgC9|ltpNC`F`s7~brh&xmjX}@TgJR1=W>zHIU<@AJ`;y1R8!%G>dh}I z$aTaVeF)1`s)97^j1uE1%8RN(%}LjQDmAlzm#)$yp6!TbeVE5FSdvoI9MAtLU6mWb z2QY#X@z?t9g?+#v-?1yOQ&V~DZ|u6N<^LYLQes#CiCy1! z-R#IDl9)qi7GexY=BgsQGi_%4l~!>$(p#I9Yp*slMMU11X6BVRWDt>y`gGYaQSNTz) zsVL?BiNC#TLE2C6D!%wn?>bAv>d%4KKfNpYPw(1Uiy>5sv*Y=O3@0G z_f>y+*UE1vhiB)&EA5QUU*44~!OVGA#mO*Elh=3goOjiq^R98HR3)Y5EpYca?+TR7 zc~?4i$&IPwKfNon_?xex#h>2QV@v&~O!0*Dt&gSCNe|lH$nIErD&Ux4VTWj2y2P1;vtT+sd zf;5#;wco4fo_3%&!^1eoz((TBEiEC(oAke{oR7nKFaacmlrFk*TaVP?hujc{9CbQ> z>UKGrGu(*2s-Yw|@umG1{nJZ09_Z@ErUxh}<@{-)<` zApe(lO|`f@=UuCA*Nq;(=DeX{&b!WjlYV(~>hNz!f}cv?d0#D+eLu0Q`&qU8s4EVw z!py=s?CRaZzWOJ2Rrw3MN+qN_h^rwhwANwYFDA(CJ_(iZ!mZbqpxK#8Tr)gY>g2nEWy3RNmSa$F9denUQ=Z?nyx zsx`ZbzywRlnQ(?*$?+OYRXhG5qusCy_1m{>gdAR~WIY$}&%ES@Dw0i0IHl*Ra$I8D z%x@l0yQQm37*<~B2g#-2t->}0=#Q$LW`!$}C`X)O2wv&}SlELxUWVt@@aBMzMtuCn zNVjoZvne}&hM|(8;z(nnilhW~9jYII9{@9oz&nL)_(@SltOEa}5{2Mo6~$db6V8ImJ~u9Nc2duy2{Xu@&WrP|Ed+GLmK)-PTNOdJVB( zibsI*H%oXrHQleQoKZ3Ox!YA{q6JR+)W?yA7C#(e1N6$_i!d%N&ydBq|MzFb7aOK$AvM|p@b_~K;(COCH}gC^~s zldhzIK2On_Qjm;@xwFH+9o5;#Y}FPSP?zzz$LXesj|GL*^vDH1+n4n8hlnlHqv!zFNNn)ACE!GyjyX?+0`8&&GJ64|R>K9_g8r zuAjTeYr#TbPP($5FIXhDos+IBFTds&M$|ux!@dHKXK0r@f9&7zFXk#=-n+;+VU}b@ z_d>5aekYb5q&CM~!Hg`cpUjmt$6OIN7c9HDI&mz!`2I-4*!}}P9lJW_m}}V;ZvmL$ z01h5VP!&4=WUiA=#>MIXV6J-u4cA9p`pI0yeHC+WwF|K6lDD4Ejs17YMTUWDw$H2R zRxOEbbIi3OLT`D*>pA9XKF3_+ZvAAgahD8xVdI#6f+zf=?|w2@Uj3iU^<$^S=9`f! z*>lWw7p`-TxuzAn^C$av_CA~fsU2%#wOJ=sgWj}#UTg&3Te;0P$qKoHJNlEka-ITz zJe*^$UBvA3C>Uu!dF&m_b$#au^|wFXQA~NX>$9$pe=^s^+u@~_-}?2+#L4r z8+qrW7lXi)^NYf$?3MF2iOzOjK-E{-(35eqVNd&kt~Z#TT@U-+mf`fTdiRMkwhZ*y z1XB;1{_&l1%e9O3<0Hf3$%Qc8?!`=SF+&M{ymR^9_iKOnoH0~7Gz`5N*Hs%-UDvKy zH*rz)rt~o(izdaN8tAT<;AjB6Vm9tajFS*gJ~nTPViRN;b=Ic2?S|8GAcXKDwcc%e zUPiY~<4oLVRVw25_iCeOZSVkvK1nnSl`B~M;%S5B;+vjsLaO-(QE1PQXRc=L*aPpZ z{Og37>5|Dc_eYi=(gVQuhOPmSQb`YvZhR(OzH~_KXyv|$ z8vnX?e){w4`i_1FkvNSU8J3q*?ov9|7*zGvBX;>(A>2B@{+j=~X7n(b%|SC`!P*a6=F4DT!PFcON&)x%>Ue=%iPb@b#xC zc$G8QXR@Th)zQ=ib$d9w&Zyg0YIk8P$G1$=Zr1_F?VsLkI=El_J@OI|v{Qi;71Mdn zokK;DXncg)g84+T&%OCAIekj^>eTlljQgQ2BiB^}@KTIX;0I$=z6qV@g1JMe)x=pK z1MCg=u3hL|T{YfTrk`WK++nvBqp{Y`fwHMgr$S%_-<=^J_97E&d8A9-4KlSgE)&_= z#D^3Mz&0QUh4G*U%p$Tq*W|VZ3{2;PCvQ(rC;qgouN>X7hhwPu6nbx!%?NfPH`@1N zH2N{kIv2j^=F!ITdsgQi?5o2W;;x_-PR4x4bq9M^)ps3|lxJ^Wb)=B;nc7=syknUK z%1UTWUQLjn4i5V@E!4w8ued`T}|!u2NZx|%@Yjc3Ot%1tCa z^ND%alXrhITNCYZpBGVsSB?U^Wantz8TY1^aQ!eBmH2#s>1a8pTEWO&7V;%)mu(CX z9}lSRTu|L~itxD{sQdlgukTtG%EcC5 z38Eq1Zo{#*Ulg~moqLkV_%c7M`R0u|$eIv0{XVJ4N7dZoRVvzBmbnONWLT|d5idX8 zMhlF!o87FoqPrz7H*fmZf~3AmJn+WD|3Zu!<=DfUhATbJF^l>#^L!(Bt8Q*hGIRor z4-(dLsy^#aYGzCwgmdizHGG4PVhmH$+`nqVkFw3YGEz%+eDZTw_~VecA`OcElYEk= znQ|GGt(j_>{wa}$5useVjL;m}k&ITvYV}C2vy-=}qg~It zD?R8^BQDqMQxKzL+BIBzoL*Q16iryW{+H}FMKv>1la)nNa^~4*1?9%<=pzHRZ8;J? zg&~n@G+>>P+L;>KVW69bgl8w+PtA2acyzi`sIIQCZ>2}*8gq@)C%vO%RU%;eqP_FG z8cb4Pq9!aZCnFWEcC;_w71Ts{q0g#lf-vuzx$9O;&?Lq9P}9&LIRQK0Pd^g&yL;Gq z3=Ow@A{BJoBCT8!(?&_XSl$FzkZnF?{_0Y2Q$Hu|PQ?3d*m3RQHoUBI{%j14qFID# zo#tg{aL3(wO7A7*VKwVi^Kv&HLl~+^`D^B`Ec~ElyDbJKTI*=qLv?o+OuvUCX<`fz zb#rOu2hdn5hpVRE(Bsnb0^il3C=xG6O^Xv4NgD>Hpc})Ca{^1*!5JPa@k2qJOjjBj z-UMhlF7+!)_E2~Dgn_qHXvs*fc|ebJzR&yXq_OU>H8uNOxIpHDxIzDAjIqz!<=9uU zPs9#srq#Jadxy1KN#x6PUoY=#s2ZvM4|=2{OMa^;0g0Yz8n$kj$FptkMW2fq(2$?X z+v?S>pP5>u&Z&?Ps#+y^Cx9pcZd4;o$;dMbCflFFFPyJu6XkUZ(4XZ=J`g8tfY&S zM3aQfZ0hU71Q7eR_|CGuF^I;g8lKyf0=6cGx3mBE`UrC+e@QaSY#p|IaWY7}H^{r# zSk-3c+PJsIq8hCV^YEGMF!M5t!t^U*cc(})=QU+vLMz;jk$u^yXWZ++M=wk5JDpm& z@lM6jf2j3Gfh-RXJ=Zo0(nK|vF!B->dQjpMEK*@JJqQI4Z!RreHFPg40r8kuZYwx- z=HG1Yl-13}$Ma1vqotr7 z;c4m6Zd z{q@DzzbNaFtaLGMulZJ>ZnM0UsLm2w?AK6>GJ3O;*US?7dJ8fx?fE5npkK7 zo_OlkH~Y#v>X)@<=ufAK9Jk!`Q?f7owTxMVy}lMSe^_Yw4??rduqaIH6f zc+hRr)-}hs0zug~oSr`xMalE;POi zjqgI^yU_S9G`DL-87l*HFBM;x!bnp?D3&Ybahr@fwQPP`n;^bRLS=P`rlXH59L*cn!sC zC|*PH8j9CYyoTa66t5w94aIBxYR=nDl=uyIYbu{U4~p+uWS^?Aow>3{{aRen?1L3< z{SwxY=98wk&D*?F-h95VF3K@mw|p<}4;{07{ryj&c#X05@aF9{o`*dd-iv|aH59L* zcn!sCC|*PH8j9CYyoTa66tAIp4aI9HUPJL3iq}xQzPkS_6tAIp4aI9HUPJL3iq}xQ zhT=68uc3JTU*a|Xe^y=#f1VA$GPdyF&xX4SBFAK_@Mv4#HqBk2t$PCo*zF$K_P(DthKe+1^WP(X4K zO`H>S>f(PLLvH_j3|YPN7yZwpZkGScF=S3~-hr+NeU9Wzw1P(LY{PXKZfeOm<4>TX-eV9 zJ2ymZSD)s98o*70eihkkflO%q?5nGOo&1}LyBZeOCRHSikvi2wVUK$G7T-&AN@*lg%534r0=}O5quM*9J#gDFqjI-8t zxl`9km@bCW;J_FrqF98nkK^ql&kP|PX}pl_lzRIbF*05|3v*m9@DNDFZ{|

&4bqo)?Bj-qTBV8VGPCTOisJsxa_|1l~2_K4jcR7{VG^4l|%_gEk>v8 zVaLc0e{Vx+Gs;5(B}s;x9!lZUT>dV3$ZmecsXNR9QdyM@FS;SqJb{yE^p>lB>-5G3 z4uXvdLk7I`T9<42*d_-(@MA56{#~{HcOg(Xs4hnu3Ed8#I6Wg?()ImS$b0d zRiub>wUhze5E#1TaD1)<7v^nNaU=Yim)nW=zp|N91eyWTfO4YUg9_DaceC}-V)R7B z#oEuJ`Xk=fyq@yA>aRyc?BE>pk%+Xkix#$|JN0JLz89 zVENl!fGK{Fl<#KGgrk5#l@w8%&nqP=a#J%tB$a*q@F^a8LzMcM>qotca|Swg8_XZa z4_qL}XC`pOIt4XL82%Kjwmsr9X*QLux{(t9AdqFKrSx{_im`-GBFIZz6X^wFvU8}S zT2qo`Rm0fF=yf(KV0e17zTITj|Gf%L@g5xehGo<2dax_4K(f9l%cX#lv-Y<%_*vym zn0tKN)=AT+;TxIstxQz<+_JU+gsGsZ@EwnxwT5|!#Z&RZiV@^Syx3!DaJ5~+*btFw>qik3(R`OuorRcJ0)8)9iL$_ zrma4K9-_19{#17}G1#)HENuPtR?=RHhei$Wb5|hCKzB?Xwf$nAcPN)wWAq96jIxJP z1Q5$h`LjA*3Uq3bOvhKrTztF;QG$Gs#EExT?_8Ma(6^hYU}AK;$tu^E7~K?jz?wzY zNkhmPF8Xc?k+d2&G#_!9;;-F6$WbiDnXo0zsxQiu6YUTf5R)v!k|=!H2H0hglqO35 zyc%cBh0#Iyj1pFL!7Mg~*j^ZxMnN-%$>~Z2T8g?HNytmorZU*hJ!-az+Kybpi~?8_ zhu=|%<6sd7DK<#S>{MV{v8*~9l}Oq2`3UBM+*Xs4d{jg@N+aGE z;nslEs7?dbcGc()s^@qJEhR2mgqxxW`HsRV0I^?*H#i92&OkM$5SPy~FriX1RXtsr z$WBm@e^Kmq>;Oou*eq!xzYku~2cKeO&v3oQ6XDByLd!A8Aua%F3qwS>SkAs!5iW$8 z!%HO{rQkey=>?l&gSZ%73ap?8CaBUb=Q=%Q!DoQ2oL31K<_4gFoVhhe1OTJZ@X!7^ z0PS1zj{)c_LNbkmtmC+ZD-YC5@ty!&z)9g@5s5LddE7l;z3|hb+=DgAe-A)^5F@1= zT({&uW}saZEEg_uMvA-kRW*_d$VNyAkl4@lh+K^<74M6s!DiwJ_kSk_G5;Kc=H^nB ziJh~lu)oKkkJ}T1Xt>J?vYRNaREm%6V533QcLhJ~gt~hN*1X004=gN}BP>&p#~5%1 zgBUNy%NUprAJ}(v(n8pwf8T?)y7lKCbX@+LKlh-^3m7PXM^1}6T1BKVF~V2mDlxvh z3bhBsXOfxZd0zQ`-e3~_Qbq^HG3YnPWC9$JO#>H-oAXXWOf3>@AjEBE zTDFonjqJ4N2Xi8r!KxI*Mi3%ah&xY*mK?k!jzN2t5&Ql-2F)xE?tuI`1dlz4z_PA2 zvD4OK&@A|^dcc;Ck!Qln8EzgNAX*XnTLMvBf_^S8K~v)Y*Cl9fyg{h+e@f6m%m2Ov zEwT>&Rf3KT$RBt`eq>R)R)S`fg#V`mty%KWjIpwcmtMl36_=o&|GNYodB|brze~{k zg0KISpuglLFUq!vOVDFs;u5rT^sf>$ugr>5r8|i`$=NC{L7$mjD?v~HQ-UU~m7x1V zueuFXQmIsPdywX=CX|A$P{NGx>|05oaV8@-4z%7iI6>IFf$Hpf)P=iq&H?# zKpHUiq;39ry!=#n#VF&(CaPs+?Oz#aW;I+|>qZhgO^x9F zuMD(S$$v7?L;szDrdBJ7GtiuRe^O(=puX{+40L$i5`z;O@b3&XEA3YXI^rV@XeU9N zuEGYW5F;x39>5FlaO)6)&F~l>E9?Xn8Z`oUrrt86XM6Cn?C#=j1`xJ-=^YriObr61 z0Wc&5+fD^o2SGAas51$=;|Bcvh?J2KtjB{`@ui#2-2G1mI>oK=Um0lDZE*(r#5G`4 z^hd5~fj9%b_)i8p=#PIg&~5>@Cd3tJ)@}OgKN)CVON}@KO}*vMdoXwAhByP=CeA<$ z8e}L{+giZW;tVvaN}Pf2L2N#J=K5L&nt#2}P3`%w4D^SVW^o3(^H&CX{KL(0W~-vX zS_WG7pA592qKnD2owpvvtkGzx40*)1`B%P$(4` z6Tq1&U1Ner4pb>;e)Eqf?a>UV5w&?S1j829milV?s6_bB;ixJ8rxR2Zu*B|^%I2(^#urZV*|9I0Iqb$0M1gOd#pO{iZjp^hn^I;H(DKm^4!obgQLFX)M zSh&~)sanp~019v-3F(2hC|l@wH01R{0lLYTu!nO$d0^`j@0EFJyaNYj#d3)xWySL{ zzDy%8P?DWh2d(MPzLS#!8HidEs*v*fkDoMyo47~#hQ@g!jTJ`L~AYkm=D;NA4(l2z-G7iF8hWlm0g2sr{Q8T&fV4257&}x+C-7G3byp zRfA+?K5%6iw22}#2WoX~gmHd()8ZI3|1HcdmH6}7NOXDjFK0fpwajzuk8_~QRyW5YjF(v_Q&x>I&|tCa)Ffk`_^Z3M<&FT=CryA`~Z=s z^yexOzB^JqlVhS2e}5c+4V|+L5DM^gRM03WPcU*T8QD^espUb;js8n+k7;i?`Zv8YpIKF*3Z<*Tp z>eIht&{=b-V|R?5rM4dZt0(e}I0jwx6t|01QZ0@_k4@{>SC5Ee(CwYGnk9(er{t?d z;T@>hWI{{aofT&I|L8#rw+*hEYN+6}7oDV*^k2&L2=0q}(D-B{!?hmt2XPNNXJD|P zZKvrMis9&F_!rAemoUtLxCiZW-#H$iY)+bc&6F{+Uh6>@SZO%N{Hq6TzFiIeuw$(U zO`k3SYROvx%w6Y}?tY6toCD4??Gw&U)8WpSr{W&8=kLm2)V_&((EZ{bG%&cyaL zWWTy?ywvvPkRidCed3qp-$ri3#vIeu8OKC5;9NLIXx~*!xmoI{^D^9NuYvx34^=Nn zc~_0?^SZ!ZBu!5C)OWISX$e?ZR(<9}KSap7v%o-=z4S`PN9ldfW;-o*eLp%{Eu00N zhqlaUte1uM6)py_1mk4G{%ymFeR8QU6I&t6{6&4y;6~tl@%Mr|0eMBl+FdRkcTzk5 zEQk6xAYOcj9_^DaoK(vGaq=0qrFbdmNmVz)?!5f6&8TpD`%7GcTVOdQL~3=S&5fun zcpg-_3BM@-eY^F0`Q;65Nw>*1{$_jZAiIR&Ps6GnM%~b`Np3OkK5~PM&a^!^*47Ml zI@^Du69v`xwQp=5hJxcO7y4&TQkSH{pmBiUlg6jzJ0aR4_v3cfpHUaIYDR|zVRopQ z;e%W|k#{IHDVWEPt#UDO2T3V6q!Q0(2%o|RqAEp>#g%y}g!4v2oc8mZfH|Y^$KwZ5gKRZSTPp}bk|iC#SMxnQcoT|FS}pFFzdXBtvr=J1 z0@f~H`eTZcK{?a)Ec$V=w91v_1ULH$@^d|ntticlbQd6)Zr2`tv3x2j%1&K5HR_9w z+P&9RFfYa&{HA-eD2bq=Y{xG;!iv(qrm0J3>DN}9gjXFn`N6{$!8|mEL8S7fu4$$! zg~}-WZsx1Lx=he`otRfSk0mv+Uhr^z!pAo>TNgE+x_o>7NUG@alOCyKj5iKW=S+T# z`9-Nl3*(gv$e>}V30`{aj`n&ggxp|wxJ}dO%moF-)5*uUPUvWbF5s-FRYTpILXMTG zF}&rXd>FLQse9&Dkmt|T?rm2~kF$IS2W=Wm%rPCW4LC?O7?yijB`34Bcp z>9}_@q@`z>nO}4Ixmj56(v@2{r)y86TsFYtfU`^uz9O;{NY=-QKZvS|5ThpxK#) zici#~bwkt(VnN?emu3{dj^6;y&M_7wZ;aWi?Et2?JD|PE-%c0U%mtz1{HTiQlVv~9 z*~#k%v*4-#ZT;C2gyVazZoyt1doaWFdlgr9egcGxnvZvFBH<$_2(1`S#ZDY&d%n2>8&KVRs1j>b@f#S54w0C(ZN{ zrhAAD07AOgK6-^8_x`O6;(T{gL(lQKAIJ1wSPEMKgDA)CQ~~bzLI~v9nO0aV!EEMU zoBBp70FD+Jot%5Ai&Jc)8Ist(_drsg^KSG60HDM1S~V8YfYn(%un;VVFUk90>8<&q zqMW4m`o$-R^~G&Q-Ewuw<*{VNSsXN(W;KTZ7&T?=?VPr&s+HPvwItQkd_c-q82db$ zyT{MOj&R*!PC1PY_b<~ShMB~xmfU)q1l-D%rn4;3yXh;*= zK=yRP9c)MA##Q4kQi&#Pdmu?Jkqo9L)rS~NL6uJeH~iiuBw;Q7*dT627c3jYAAgE6 z9JTK#@ae&?wW7b2+82C0JGJHSy}w$~%hq_?zu4Y6;#M?tN$};)ZxOB1@tX7WjzZuf z)*AWJ?HK-21K}jdsA^xu;dQb{%ly^NMb7HhLX_d4$%}&hx1fR4K_kU@>4m{ZSYENS zwfmxBWL|v4rh&HnK@yq8^V)IbQL@7vRr$&O@7R+9=p7Nm)rah`X^F>DCf_~t*2OV5 zWid8iO*->#OzdtnB{?@VB<>KK?-m=*eqhZ_9NXlA$__&0hSGW$Eu813?>j#_#_)%m zI|&maz_EdBO+EPq8WCEx*^3d|KYbht5ANm|d?TrDWyJ1LZu9SNageo{5C3;7dh)rE z{pD||+P1VJ@hz(@SLMS+R-5;zxE1YU#eL~VEs2I0)^?0#;%=*GuK)V)R`ecgmjuln zA}4YAI9TdeEBeIjLMi$>>^981^+`Yq!>q)}JrsppC-J6t8r(R<1I^X~Fwrk3U z^;O!(cwZIvILVVJnFvL6R-X&jUG-h*D_0boMzto2ug-3-mA9JF zI^0@3SF|2{dc+1=#?!A_U!r4hnhYH)t$WiF?OeL=Q+v0EmGQp9s@6caKr)It{PFIF z>u!Y-^p&75^1(8JeXaq-9QJ#;m7wqcYxI>gmd|52 zB&WHJx~%IXwnMA$or8bhr7iWf`tMV%prY(PQ+`nU`>da4M}*cfZ%?<5%IHJCg#+wBhVkXlE6rJ{1MBT|^GWs@ywEORw~13A6m z_#JIY>*LPe1t9anDr40geeZog;vDbJe?D`QI1GL>Ym8a7fCL}!xFoBk=$JO7Rj@^6 z4&LsH*2TY$&;OLJq;(1Whl^T$Wp_mkgWKZ*Cd5Bp-rs)9TS4{eq_J6rRa}=$#cnfO z;j@zzZ_SH$`=4qZn{VxPd!j5-Ef%@g*N==<`?S4y!%vgURKIlfMq>Te!<+1gZi*m? z6E}YMAeG=5dhOHdkm&ZlkM^LIJhYq4es;fxlUsORf|(+}P45<6CUC)t7kP@?nZ5xV z7X>z2&uTl&6Dgs)9Z5dn*IxUz;;)z%*aDm!B4hFt|4i_=y~R-d#qb28Kr&OE5&-_J zwBhI@18%E#u4JZKGE*&?sg}%COJ=GiGu4urYROEsWTsj&Q!SaPmdsR3W~wDK)smTN z$xQXxy@w<-)smTN$xO9mrdl#nEt#p7%v4KeswFek|BIPw30_O^8mc=}U8C8A!n$Ku z?_5pJ&q(q^%9Ac(osy3Jog7tBvbf_$hwCC@7gKv=D%p0t2?w$yna?I4gjuPfXkY#2 znr^;rt;}1Z9M7p#yyUCLdzqn-3eG(K!6;=q)@KK4vXTmCf#OuD9?DlTV>1fUY=cKAdM*`O)`yUYx2Ym`{6j zQ9-c%qWIkuuFMaYVw7)Qrp^K_R)W_mzTzcpP4N;op!{<@jcJ&*?Uu=GA+3fFV zHa)KVRMA0yHVAxRIUL%V+>U-`2a3>TMJt>^p~eLgypCRpYIQx?Xf2a44&+GiT7uUS zyq4g#1g|A{Ex~IEUQ6&=g4Ytfmf*DnuO)ab!D|U#)35)O;I#y=C3r2tYYARU@LGb` z61tBuKFKdnE2FgE;Wy#5F30~hzlzqU=TYbLWc9=&Smw(;xvFWJusSn?t z$#PP)cW+B!|FuqUc@(FAkaVZBFUd4LGJ9vp)2K~4VYeNJ;gLxfi*1Tp)845zNXHX3 zcHoo7%r+QRTrSwbNgp2ZlHj$OgERZQ#(G3Wz#x2!M#^EIU{t0bYCtf%JeRD?Wo2lXem-v=-gi! z=SS@+pCx$RD}+#ozy>UtB=VaR>+qybYojMv>D5pYc#59DxKSA|C2Hp+@9D9oj9VP? z;^R)<43?8^g|+Vf4$2p>q%1Tte^f~y74-%U#U_@g4fE>d>q4p_NyB0BEf43UQ6&=g4Ytfmf*DnuO)ab!D|U# zOYmBP*Al#z;I#y=C3t<#(^!Jn61|0oyi|!@rv7%@LI0G z&ivj=i|zUycG)6gRwWNIQFpGmX)j>jRU~OeOIp#pSgAhi6n~2-X`J1(rM+4tg^rJ} zd=!~h>yq^jC;#d3$i^nmBg(uRT^8s`bW#t<*E@}&Js*@Axya+GI5P~H9mfP<-UV{C_7B2k@ny~B@ks>Q{w<@|kO3!HJ32dG|); z52*u?}zO3ur{g!=~YN25Q3s(uOA3t$&#uD~g(u$U}qHE16xzKRM1o$;UyhA*n z`%yqO2xh}hFN0;SN?Os9RLSD>QfK_jZqK*EdKQBGu8 zI+LoO3n_H@q-EtbRZ>{W!AvuO+BTa54z)RLN~F_L*d)a^R4lcZzg^|ey??B}Gwm?m za#|L#J)7@aZ?XH=#`fyflO?}4w*6GyJXu*qR}gl`w@9JLl(?U#DQ8gkE@!)n9y`_o@WLfvbSa5yQ%=`opd~bZ3 za%FIyFLN%R0tCWFGTd-eG7`)2#u6&A=9dcNI z%0f-1+0diH^~rWue&+H`@AGJDd&>p^8#{S4Om`;FmgA5Ts1z&HbY~xGGa+dmHvv55 zP5m~Fi1*apalTZT1g-%r0)?b=lmMjDk+BDVk1-NnxsRDOqEpK18X)JHc9-1uG2 zD`oZ%$ioEy>q5Gu9IubjBhyr_k9qOS9mS_}K~A(b{ok*UWz&z$ybC43EvTV#LyB+K zVT)nDvkmI8vAW>>i&>Ei+L)cmde|nq2U|^!`7+sF-^}-c{hSu&9wPzw$K%YSKplHSX9z# zSvwMl4xpsYlYo5XG<*}5l6u1~2Q$Q%G8oLwF8)#Kv#T3tFvLX1go1DVRP2O16es*# zt)kv2{PtE}Eo<|JYHhV2CLe5uUm}J)Lz)sC~(2?lFLD5)X*_CgA zE)T@^r`i$0c);-1RA2k#%`N&~H&V*{(_}xh6y7$oRrw6Km3A%i#15ku)F=$d~Z?3|dfRKu%7>cb3=D-DrG!-MA@%ZH4ZqB9MT_>1)|0&86nq=8 zteGYC{SDla9wZ;s{K!21Ui_9p8nJ=rI5iB~ccDjy>^)uo?HM_Q;Tgf|ng zPVrg!$)w+xIGXV;0){BAc9S2aDOBUc$uY zI=<2@B^J3mWb9HNOSugXCUR+G(%YkL0%|?hzo%|!GgI~1GK2$lGcbO?Lo5WKw6nm( zhi!kv4=I28OXF2gYkV2Y@Rs9-4GR-5UAMPz6|m@ab9Q-(uN`gPTqdncJf-vJ!EgEd znSk|B0s-rCk7CIEySZz03eAEVF0;Y13los&p8Z2ZjLPYI%w*|oD-;2(KKuSa8 zg)9S;tPAcg_DWZp!A_iqnYpBW?i;f(t5_pZit%1M_37;o7r%kxobHu`=!UM;S+Zd< z4!rmLyTvNY183pJCHTq9F@_YDAs071h1-A8W&=mo(>#ny!b&k?rk!C|L{jn#Qdg)j zSaQsF5-y7_dsuYlBo{s7UdxZT32ODuu`2TJsVKmZV)gxZ1t#nWYv zGZlFi)<}pclmwdMAQu3zBOldS3iF&s)zG0ND#C{oL*bs2r@+FUQOm5@zh509wi(Gp zz@@)nTyIOQP~ah4oDT(w7vMgY+rE5<%jbl@t&jzg+&@u(Auh(zXP2W8sk|U1!#}^x z85J&srqN>w4A7a?ouu1eKs35$cO2~Ry+C9w2UL-a5H!?8~_Z$ze#-k(Xv9g@tGn-`O87azNLHJl}86l|I6q3*l+9rb5;FGL< zP`B6w_e(?^9f-OGGpL zlsDvMr&++Sya2_CWLPA`iU{t=$qX)ok#}^ zC;}_uG=;~)+Hg&x{jBF8X%XZN1LR9b-5!J@@i8jPwa*a#kd=eW7_|o5#p@NFRu~sxqA6Fc z@I~sAxUZ3xZz=HSGX^UH=u8>Ri34k-c=l;38xCcL&A4UeWmMz|PN7NU$_1bG1(rd0X!tNWQ%m}q!GC9y0~2Z|sE+2|JSto} z_BzukjaROF!-ME?!6Q`2@q0;8<*+??z-O=a??b#ep7iSuRd z5x|Q0ihfi~tTLgtq?<;^LFcGIE`tY`hK&j<{OJu&OWaWarecL{WLC@Y3XAW6?XCex zplHz^MMiAyT)d`YETcos|KX~p>n}aP$2c6Sr8AN0Pt-ONq^-}Cj^l41*T>3`K+*eE zjGn>uISoEH;7OCT8j;Loon7p4bncHxIb)eqSGfUkV0k8J0w}T)NcmG6wa?UgxZQry z-IRX=6hXr3Gj9Hztr+EDhI@Q%sX2k(U>Xxn!$Y-N35xTwX*bNtO<;feRaQOb-Jv@^ z*BVro(2Y!_0~LVcvFAAV9xG!Xxk2|bp;KELFW-PW3Bi9}-TD%ba0v+0dDarT{_e^u z0KI{4O8*Qxp#e1$mUjrGu!|KBmL6zJ!#-O*w3mMPe5sId$gxHw6?@I%0MC@hi55yj zE9@TF&f=Qg&`UYjN2pb`haTe55BaDYKa`zS1z^QGlW;tWMh8df-u(!HmOZYSy7s_o zZ=D$fy846v{X_Mo!%%5A^iZR#0{TKrjs?o@4`NpEJ@dBYxZGS0%=wP}{W4ew7mU7( z84*a0@OjuLko!K{6?P-$QQG@H-W54|l|5N1^dE976kCIBygz%^zW9d#!ttR9f2vI=o= zV2WG8wMuwd2P%isp;iVv{38h>02hjAZ@KAlEw-?{)@2^O@K#x>by&MjR|c_1NlAJ_ zbbFYED`fTLwnJF%u+n8(fCvh+K&ttm_||}~87tIDt&xb*0bJz5!}kZ-&LEq#?6X!V z=u>D55*3!mm$t8g4Fli|R!ko2Np3r8ffExZM7=JBLFfs2%obQ)i;kbyk_bT)?3WvW znO;yFt;apvZOU_o`tu;k4)7x`2v>4KArCP-)sxo?IlDVyb_$j#M3o4UvquE#X5EVn z^tAwGTLP5lOSpDgnOCz{d#~A@h0Y@|3y4UdoEH-kQLz3 zXYVn%$VGS~wcmSjiQ$A(coVAK=1fzHKQX*=kQ{Oi=zR^^#C>7TDLudEMN4QKGw==h#ab8ZCU(^z_G#BgG}AcvLY5gW`3j#|Ara;i&wr@l=)%9YW&wu|g17 zk0D2nZy^f@34Idn-T~_#qP8h=R0&(zT$i7=;XKE`=MlO)>`e5rbkVciQrJE;sGRjg zm4yu1%Fe?V4GU3L+@Ym*)O6Cw?Nv>C87M(@2N+hP_+*&;@%ZZq`|8UZKm(%NXUZ_^ zKElj}utE}Q*wE>oU{n#Lo&Fu1@&V`LXzIy)OTE>qO$F;sv}fD^(HU(wpnZ)6o=Z1z z6@ou+XJ?rtH}0h9N9`j$1$#Mm_Ef-Gqh}OoW>aRk>v>3t9|U@bO-HGcs`7`LWcj zKSG_PF7{6DoE(^<%YN&{?PBEuo`kKmxB8}BMOuZ7*N66Ro77qTJB}Jh8bG5NQ-xF*o<0Kn^`cM!>cu2A!a&$`-1H}!&7ST_ z!r{L@;kDSLhpRTPy6v{UhQxglknBcd;yptFkl>#?#oeaHf+kdY4-dxp{6lkcTkEfZ{*CsCeK^ z&WPDC&-t(}9beo`Wm|-ktRH9@n7GxK+_WW8p5qF-CftW6Ke;G}LQuG@Jb!}yj`asP ze{|o5)s`8NPa;?SM}~pk5AcDrFNSTmB314?4{>4+jSd-k6LPfvTEh3(^wNHQq3FK8 z?XT3giOSkU5wX5Jx!b?=;4hb(Y9jNx@eeaf#4(xO9m3sn&72Q~ucQ-5d{p&lUo8(w z0>CA!%%7oKl2N#;>lg7xUV6cQ7&WzbZrKL-NdFmCYkMp+rq8H;h51c}068&FochBE z1>ngG7;Qg+kQST2th$Zc{ zPHx_DqxI%)q?*WCyVbu`(P~g|(r1L#;H=do92(#89PCv*%dOB*!IG%5gR)OH_FX>b z+2l>7*)f7*>XuJfxrJ2fZ;5X}pYsl>f0Mk;a-4Xj^g>{zd(9^|l?^pRSC&{%q+BX= z72lRHv7klu&rB`0E}X9SD*NaVwry83*f6X1f&1=dW4oIQ?1x9FYWw1>IkDiEV}B>i zs=fZRzCL<9IG%dDQyJmN3(^PM&1k1SU3xvW;8d18`!yI`X2lxP++1g4p=AwwJdsqR z-sj3UxqXE0S|o3BHpAlB*AEAB(z#1PEr8bvse|6mzN`wuns}*Eo_q}LJ3)T@?e?KBw{Wt#I-p@>J`Z_4Q$$VrM|e|pwZl{ zJb=RO_1eBgBi3Sf&cJWAy%%}KNymP>oQwZP7%b9=-l_{TNH)anlos6Z_TP8f&p}r? zKU-es&`rZM1L+J8KH^x|J5Y}F!v@CgO3&oZH`jQg16XC9x%CfEp8u-t8{+Vc9axDH zO#x?HZ;|?9`WH7d+~Cb3R2D$B&{I7wUQ$Mb-xGqT7d4czTomgxf3DxQ9NNt zurBL47)&1Bxeo{5=}HI>sx>M|(n>bLCKyb|nViUZcgJ$05md>&_`=>*O*jXu*u-VX z>3xCP40@2RR$Q9^T?@OEUEWYF{jC_NIdN;>r#l-3TwSN*oN@MfwdwZXg^wTHdL2Re zz9FHhs}o>}?_QVZBlBEjbadZGNjbO1nL^do%9VA)1vlDBo?#}d2Z`JJ?l99bmwWUh zhe}bv2M71tp+x-E7hP&Q9BxtbhT<|^gU-twY>QOe0%+t&@1mD|Q(2IYS%#%(_=y zir#uTrVX-FHi%)h#aYWjASu69_YWF#Z&6_8lK0Gf)U+Bq!;c1*60GIo6fjV)-EW)-9DA2c}nAA!}u8PIAVjFN)YIToqr>ogxoe4#j-ubFWns4j*n_C zZU75$MqlIhS!Z{tq@I2fIDZr|qd3uyYAxNc8VbV8=fT^S*-0^s89TPfd2_LUu_E>93&)bfHYD)3+=sQ9Co0_z36c^Bq?wMpkQI?mUq%Y*8D3R+<_;Z zgfetI2KXCX<{x6G`}}IH^pMa`_`|RNBlU0TNhI%6x*=4yJS}< z=LfJ#zRFK>kOfW9lXn~{wbC$QSht>S+%YJ1D|A#&P@Q?isY4|$Do>VP!jmp*!#lbJ zB|eMxD`&K0SEgrS@c1Mw|H7(sB)4tP_is-cngA$Z-?68}S4MTgUPZpt{-DR1;b)&i z31E3RNPzp5*l9qAl3;Hb4}mXEdWuiF71F}DW_4#d{!9Y?t){ zx3Noh0v}kALjMxX#e+%b1J;LkjNz`IT&L&yB^EQyLT;DUu(ft^{NPQwlidMv)J*Bz z5F?u2xLArFNaMl02X=3tfk5wO7Z!SqGDYVITP;#4X)d%8CNH#cm^<|B-X^5E#sXNEw3uItp7_C##{(Ka zphy$RQC5j+k|xV)nxcpFEX-E7l^xPK?)o-J$B#8wxuV!Ksx?u3b#{BLyw!}>;nw20 zqV?d@BR0@7o_^K(5*>rnWawCF-J6zZ=hA(j+PgihjQ15*wFbHcitho3Ki=JN-K{W4 zqMHAItETONJBEb+6V(J(SNZ=3s%bo{u=7pPh%2{>_(EsHNlU5W!S!2XR7h#QDyok> za~pv^7Wg@x3Gh_Dar!Ylva(Cnvwf4O{i8A^UozF&+gE5 zrcyw!lqt@1TN!P7*O2MdyKz?RvVrlrEv_kwdX=lDF}>b}YVPl~9@pbk{=HRf;=$U) zQs31vRs1&Y+$DP$tHF#(E4W{*4jB zJYW$o@2C4b2RpmN2(oR<*=#9&MLGDcxCz@#Cu131H)M)kx67O^(kLxnz${x)9i%mD8{@Dyaav=xoG?ON_G}y zJfzR3{n&3B$)j)HfeFbq?ypYg-TUo;PM~>~sk7VAI!3Fy5{V6FF%zx2gR~>KA0~pQ zafvmPTWUXi(l|!PqRiqN6hzMEk3;pq3lN-pLgj{^i0(_xlVB=SL+B9X_iS+jC~gkg zw5$QSauS6dn(op_%_ipQvSi-5bZR^ku{U3Rq245f+g)l%wS8=c-Q3oiOvxq^CJ(E7 z$K#Pkk9|&ra(Bo!kJ+dNvCZ?oUUlzfd&V5xQW=S7n)thTrI!-4CaJO|_Rug;o^qexXu2`o6;7$g`L%uJ&N%QVxtx!Rz<$3nHjVbLx@mFjY zMGy!xXmeBEUpAVQ_JXh4`;%JPhRb6t{d}+#r8~Ydhzj@Tamg{*Qc!R!&Q+bI-sAC1 zkB9$V?PM=lh2yyXcoy=Yix$zB?^5&kyi8fzjLxmrR+x=(qWkp3Ye7bn4MULZ8)*P@ zoP^3R5L&e$e!pli7^i~ zh(3(J>pyB_y2y6+s5it(Y4VR^(B8(yl+lwe;$O69@mdBhu@4SY00fmPOG*MXQZ zj|7}XuEwncX)`n2g%GGC!*RZW8sW<;L6At$mLg0XV;4WJOxV?a_uLq=&Mo+qpWn)@ z+fwIygV2lIHf@psrxwM5zZAhYuy!9QhYE8)DFT}sygCNuTZ!QFanO!)mfjt$$TUV2 z)PM_0`dxTZ9buQkNrFWO9#dFZU|UxTkvq#SCrR9GkKG!R31WJgiK^w9vjdlGePo#vwz6{3(FM#3(K-ztznt|zJrS40JW8@ zoJ?55GH%R&u*}g{89T)~z8RkW-&iI*mwgHuH|Ve>i$&wXevbp^a|rqZDQ`t5Ys&F+ z{4!4p$##3ZDza*ol64$5R(dE^K;Nf{Fjt5*`NQBvYfLW3$oTyGDdZQVkwJ@`Xyz|WGi~Q?j-IcXRBDsZcndC z+53N_3|Nyg*$pvnS!q-%)!ZJmXIc|V!A>Y*MtJtEB+$6XH#ZKo%7(={sWpj^U1cyc z4s3$Kpd{*zjk8Dt#-6mzKTT3j8DAJBWsa^gpn8nVTzjlMEB*Czka!+ewG>pxJ1fJN zP9FqyP$6F|(AXh3tj8jeZEf65Kt-V0$re+m6ufnzNG=>nh3GMpaw8xs%%~+IT9pYY zqoQH~PLK~uRS0&XWco8@R`{vWJj$miMeGzo_5>)CDW&6Mx~CZor6|==foN)0G#73r zb{hdGnTir3=&D@I$t$qTAL!_Hnb91Wkx1R=4sM%ssvJ#mv29~=92k2FO6P$=FV)tV zjFkBwCbLF5xBSCo_$zBnM!zOzvY>5{y|17Ff9H0*WXS&CnQWDN=3h<*T#4pi`^U+w zc#wOPqOUy|D=t`%hcRR1XT8Qukf3`>*v5mPand!U0AqF95=%C?6&{%5MNkbL>`{P$qKvN+Df<3AzF!UoH7EgrSRyCg{?sx}bi)WPN%Gk^>A@ z+Lno!%($zi-2^Q!aAkAB9 zU$VIxp7>uRx zReWgS|4e1fYV)Z^ush>|kK-1~`aWEw{odrIhqimoP22OX`Bfo+zZhAl9nPEr5#F z6Bdr}Fh_ZqPsk!`uG{C;i**T<0;>1=_5gy$2lv@w@(Kw=XJ)bLk?x(4(cAVHuGWR% zYlp_`EVz~!a(W6O_m@OQdKKbk0syL>-bocxek zN5O_vV-|$Sb7w5A=}kVYX7w`IcK)#(A+4O=0^!)r$}=XttcvO2^F9Qd{qo_Fp9_v;B7ef*v`V)}_XDn-2IKg+RTF%x03NW3BynwQpds&Hwd%4L& zrFIhbtTKv|2evLT@0!;ibl||Om@bj{6Y)TV#T4=a{%G9DZEJj!75r!*15ryt6^ibC zSfYjAz&#S?HqH}iyn}M{Nw>Z3c-7&=T2|!Tzz}O0^&uF3G?H~sjAbDezpza3;Mw&s z;O~1+^sVOhqk0f)Sf(z$hGp1gi}xesQmu*=e$DTZb2!AEZBqYr)C3@L!e zT*vmiO_O7?zZno@<2$!P9i$H6qg?AAKh;!3zt3>#&NcBvPiH=Y4H~^D7YZ<_8z|&C$ zBcMD^hgTBvdnKlp2MNDtTO&GBbXMO)n}#1S*5TBqj$KWEW)x$+Imt3q29F7;b{-Z! z45UK>g~$?oZ%~JHM;d|J#@HyhzQXY_n}GJAwO3rh+FTPY{R$-jhP>w5`K0H)cHbfs zHF;Ja+pQpLER#v^_8Q}KSGV6eT~~I=jwCu_Qwe%!@EG5Um99?%8hUBF$^x#zN=5{Z zU!nbvK!*7i-(=cx?2S4V1s%?rtd zE;c;bP%wo`2j@F%C1$^%9uM5hDlK$7p;+E;?E~$<-lQ1^J{|WAYi#+Oa_fOZ=Et5k zVcpe^d%Q|&XmM9|8m2Z!J0SXZLRRO<8(OauX^Ni31w@nXE;=}8kbDYwHAx#xiMAZ3 zBEk(&D%+YM+>6oLOiQ<}Wznm*W`xK0qS~^pRID^~I75w~=V_`I7x;nzI^^sVdy+`% z)e=n=IGUTAC<`AljtvaG`3uYJZ(~=R8=ZEY5Np|mfF`k)1zxiYJr+|C6R&(==HZ^m24f z%dX$-T*I>VW?F&Xs3mr_U@M+`?1t47F_ziZ;Iz-aUBfa?1nBQ0=$~s?)-R9DO(%SI zBAl;W!?J~+|H86Ziv~JlP>f|4=eosMwhj$C?B0=a(;4^k$OhW^{-`tiKjLf$MxBW8 zwh5~??_!0k-HQ@_7R|~c0;ozj&0(9oQ92S-Dd>ioMFIM zk?#mu&J`LRFri9^2vwROMa>RpPk&Y=Y@yh(MRu@EoMIMa4~gK;7bUT;r)z0j5xcbk=ntch>mN_f7(LrWhkifY zJXH`Av2x@JH8zcRwH5-+s9+i*07(AEeTvG))W06suN0^tKN69FZF_8WcL z{!j~uuSW$hZ8p&H`lHv+CJmX*_YMdcHK_sOP`!tOgi?auX2?IB@>wp9YNO8{`?{M zSJ2==ga*6F0RrUAu@ZM?#G$~?;zf)ZHEzsE!GeWxdJ-hiqovgLGKR=EfsmFy&q)C;YJ~aY|;G}vm1(u*$wOaIO;Th2c zG?tDfdy>bZr#1whsM@yR)va+i9;gX6fswL#lk&jyR9~N(ZUvu+CD$-NEdJ&|0o#p- zUdStXczW8xm_QwZ1sCvl;BZ!t{(dkf>NxUglmq-;5ePuDW*s>+c>b)BLjPi~dPI)w zW)k&j)lZ`id|kYNh1s>aMyN?(oL#ZMPg8!XXl-%q|I%6T7vXRmPBr^z*4C_p|7VzK4H2QSb-Ab%Li zL5c_dd7zKdRQL}v2%L)P9M=3v0YMZG!T^92a&Ym*beQlVAt~l~hL{iY;=vGn{L+Vu z>rxc*Jq$$iCdaWh@#Gx8#$ZIC0)~__y8z@%1WBYe0YSu=>@h_k3xt_+Ozb%GCy^$w zM2UwEK*+?e78H51y$s2;Q?me45aC@g9T0ucHDn* z@E|2WCD;~^D;gQ&4LVwohl&>JZNY{&#%SbV>FmKjN*fG++K=FE#Bupy>5 z{Bf;gQVVb(lP5w}=@XLQn;DdTJi#OrTC4LX2A)YR!vYv$qW@wN5FmJr=m6e}#~*Td zc*4i0SYRY-G9U0^Z6DZqz@Hj?d_x;U@DP_uf4Jx;I3MMi<8HLy%W#NvB<8LVAD_?y z5DVr((H}hefr;!u{Bb;R?9?{%VV38>!vGdu@ezc6$nj?b6koU?n91RM#h(`tK%fT^ z{$U4msva0(TMHz3p)+8L2 zY0<$iqDaXNxYM<-Ex;^*A|w<~q75eMqXz!yg9lup1wcWPkS~HqfpB42+Sma$ zfCM=`l8+9P@XaRF^OPE-1P~YSM~3voh(zo_8?&V44tCH42lAkGR49b1`mlf-$kLGB zNJS8%_5z7js7IoFf&&dO2$X1pI)#D&N`i5LDE~b`KFJhF3X)-nlW4(+)Ef>r{lPJw zZ2$od6NoYF_mn4k0~Fk`zz>>mq<-)~6qpF(B%sKF0~JIJJ?KF?dcXvz86*ICLy0I{ zCy)yKLj)MWfG{!>z=Evv6&-v+vzow4f6Ne*k^+Tq^7$LO)7j%&it=o5{355474b jcBs_?3!oGtXad!|8l-T!dVv&*8o;SRj090b3J3r@AnA^X literal 0 HcmV?d00001 diff --git a/ground/openpilotgcs/src/plugins/coreplugin/qml/AboutDialog.qml b/ground/openpilotgcs/src/plugins/coreplugin/qml/AboutDialog.qml index bd3d9c9e6..292cba7ba 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/qml/AboutDialog.qml +++ b/ground/openpilotgcs/src/plugins/coreplugin/qml/AboutDialog.qml @@ -57,6 +57,17 @@ Rectangle { } } } + AnimatedImage { + id: opie + anchors.left: parent.left + anchors.leftMargin: 10 + anchors.top: logo.bottom + anchors.topMargin: 10 + source: "../images/opie_90x120.gif" + z: 100 + fillMode: Image.PreserveAspectFit + visible: false + } Rectangle { anchors.left: logo.right @@ -76,7 +87,15 @@ Rectangle { Layout.fillWidth: true font.pixelSize: 14 font.bold: true - + MouseArea { + id: easter_egg + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + opie.visible = !opie.visible + } + } } Text { id: versionLabel diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 9bd98c1df..6aa002df7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -348,6 +348,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() } for (quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) { + data.ChannelUpdateFreq[i] = updateFrequence; if (m_configSource->getControllerType() == VehicleConfigurationSource::CONTROLLER_REVO) { if (i == 1) { data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; @@ -357,7 +358,6 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() data.ChannelUpdateFreq[i] = ANALOG_SERVO_FREQUENCE; } } - data.ChannelUpdateFreq[i] = updateFrequence; } actSettings->setData(data); From f4ddbe53eb7cbe03e85793a063f7a857a4e73777 Mon Sep 17 00:00:00 2001 From: James Duley Date: Tue, 19 Aug 2014 10:41:24 +1200 Subject: [PATCH 160/718] fixed whitespace --- make/tools.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index d757091da..8213dee4b 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -597,10 +597,10 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists) else # not installed, hope it's in the path... # $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH) - ifeq ($(findstring Python 2,$(shell python --version)), Python 2) + ifeq ($(findstring Python 2,$(shell python --version)), Python 2) export PYTHON := python else - export PYTHON := python2 + export PYTHON := python2 endif endif From c2af28246f5e7706d37120e24018af356c41ebc1 Mon Sep 17 00:00:00 2001 From: James Duley Date: Tue, 19 Aug 2014 11:50:03 +1200 Subject: [PATCH 161/718] redirect stderr to stdout for shell python --version --- make/tools.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/make/tools.mk b/make/tools.mk index 8213dee4b..ff3e43561 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -597,7 +597,7 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists) else # not installed, hope it's in the path... # $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH) - ifeq ($(findstring Python 2,$(shell python --version)), Python 2) + ifeq ($(findstring Python 2,$(shell python --version 2>&1)), Python 2) export PYTHON := python else export PYTHON := python2 From a365c5c6dfdc97ec6cc0cad92736c00770581ccd Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 19 Aug 2014 03:14:11 +0200 Subject: [PATCH 162/718] OP-1438 VSI_Arc_PFD+Status : First commit, modified Vsi with arc, added foreground layer with gradient --- .../openpilotgcs/pfd/default/VsiScale.qml | 105 +- .../share/openpilotgcs/pfd/default/pfd.svg | 995 ++++++++++-------- 2 files changed, 625 insertions(+), 475 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml index c40591424..3f0f901a2 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml @@ -13,82 +13,69 @@ Item { x: Math.floor(scaledBounds.x * sceneItem.width) y: Math.floor(scaledBounds.y * sceneItem.height) - property double scaleSteps : 8 - property double scaleStepValue : 1000 - property double scaleStepHeight : height/scaleSteps + } - SvgElementImage { - id: vsi_bar + SvgElementImage { + id: vsi_waypoint + elementName: "vsi-waypoint" + sceneSize: sceneItem.sceneSize - elementName: "vsi-bar" - sceneSize: sceneItem.sceneSize + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height - //the scale in 1000 ft/min, convert from VelocityState.Down value in m/s - height: (-VelocityState.Down*3.28*60/vsi_window.scaleStepValue)*vsi_window.scaleStepHeight + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height - anchors.bottom: parent.verticalCenter - anchors.left: parent.left - } + smooth: true + visible: VelocityDesired.Down !== 0.0 - SvgElementImage { - id: vsi_scale - - elementName: "vsi-scale" - sceneSize: sceneItem.sceneSize - - anchors.verticalCenter: parent.verticalCenter - anchors.left: parent.left - - //Text labels - Column { - anchors.verticalCenter: parent.verticalCenter - anchors.left: parent.right - - Repeater { - model: [3, 2, 1, 0, 1, 2, 3] - Item { - height: vsi_window.scaleStepHeight - width: vsi_window.width - vsi_scale.width //fill area right to scale - - Text { - text: modelData - visible: modelData !== 0 //hide "0" label - color: "white" - font.pixelSize: parent.height * 0.5 - font.family: "Arial" - - anchors.centerIn: parent - } - } - } - } + //rotate it around the center + transform: Rotation { + angle: -VelocityDesired.Down*5 + origin.y : vsi_waypoint.height/2 + origin.x : vsi_waypoint.width*33 } } - SvgElementImage { - id: vsi_centerline - clip: true - smooth: true + id: vsi_scale - elementName: "vsi-centerline" + elementName: "vsi-scale" sceneSize: sceneItem.sceneSize x: Math.floor(scaledBounds.x * sceneItem.width) y: Math.floor(scaledBounds.y * sceneItem.height) + } - Text { - id: vsi_unit_text - text: "ft / m" + SvgElementImage { + id: vsi_arrow + elementName: "vsi-arrow" + sceneSize: sceneItem.sceneSize - color: "white" - font { - family: "Arial" - pixelSize: sceneSize.height * 0.02 + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height + + smooth: true + + //rotate it around the center + transform: Rotation { + angle: -VelocityState.Down*5 + origin.y : vsi_arrow.height/2 + origin.x : vsi_arrow.width*3.15 } - anchors.top: vsi_window.bottom - anchors.left: vsi_window.left - anchors.margins: font.pixelSize * 0.5 + } + + SvgElementImage { + id: foreground + elementName: "foreground" + sceneSize: sceneItem.sceneSize + + x: Math.floor(scaledBounds.x * sceneItem.width) + y: Math.floor(scaledBounds.y * sceneItem.height) + } } diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index eddbc6a21..999e32c03 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -7,19 +7,42 @@ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#" xmlns:svg="http://www.w3.org/2000/svg" xmlns="http://www.w3.org/2000/svg" + xmlns:xlink="http://www.w3.org/1999/xlink" xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" width="640" height="480" id="svg2" version="1.1" - inkscape:version="0.48.4 r9939" + inkscape:version="0.48.5 r10040" sodipodi:docname="pfd.svg" inkscape:export-filename="/Users/muralha/Desktop/new PFD ideas/pfd/test2.png" inkscape:export-xdpi="72" inkscape:export-ydpi="72"> + + + + + + + + @@ -42,6 +65,17 @@ offset="1" id="stop5010" /> + + inkscape:snap-object-midpoints="true" + inkscape:snap-center="false"> image/svg+xml - + @@ -149,12 +184,12 @@ sodipodi:nodetypes="cc" inkscape:connector-curvature="0" id="center-line" - d="m -179.5,169.08132 997,0" + d="m -179.5,169.08132 l 997,0" style="fill:none;stroke:#ffffff;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> @@ -1137,7 +1172,8 @@ id="layer3" inkscape:label="info" style="display:inline" - transform="translate(0,-4)"> + transform="translate(0,-4)" + sodipodi:insensitive="true"> @@ -1204,22 +1240,22 @@ id="gps-sats-label" transform="translate(0,6.488519)"> @@ -1229,12 +1265,12 @@ id="telemetry-tx-label" transform="translate(0,-3.08992)"> @@ -1244,12 +1280,12 @@ id="telemetry-rx-label" transform="translate(0,-2.08992)"> @@ -1259,42 +1295,42 @@ id="throttle-label" transform="translate(0,-3.203204)"> @@ -1307,14 +1343,14 @@ inkscape:connector-curvature="0" id="path4614" style="font-size:8px;fill:#ffffff" - d="m 632.01025,27.035233 -1.07031,2.902343 2.14453,0 -1.07422,-2.902343 m -0.44531,-0.777344 0.89453,0 2.22266,5.832031 -0.82031,0 -0.53125,-1.496094 -2.62891,0 -0.53125,1.496094 -0.83203,0 2.22656,-5.832031" /> + d="m 632.01025,27.035233 l -1.07031,2.902343 l 2.14453,0 l -1.07422,-2.902343 m -0.44531,-0.777344 l 0.89453,0 l 2.22266,5.832031 l -0.82031,0 l -0.53125,-1.496094 l -2.62891,0 l -0.53125,1.496094 l -0.83203,0 l 2.22656,-5.832031" /> @@ -1324,17 +1360,17 @@ id="waypoint-label" transform="matrix(1.0375459,0,0,1.0375459,-7.161678,-3.5667345)"> @@ -1344,17 +1380,17 @@ id="waypoint-heading-label" transform="translate(0,-3.00017)"> @@ -1364,17 +1400,17 @@ id="waypoint-distance-label" transform="translate(0,-3.00017)"> @@ -1382,31 +1418,31 @@ inkscape:connector-curvature="0" id="path3906" style="font-size:8px;fill:#ffffff" - d="m 290.04935,24.257889 4.93359,0 0,0.664062 -2.07031,0 0,5.167969 -0.79297,0 0,-5.167969 -2.07031,0 0,-0.664062" /> + d="m 290.04935,24.257889 l 4.93359,0 l 0,0.664062 l -2.07031,0 l 0,5.167969 l -0.79297,0 l 0,-5.167969 l -2.07031,0 l 0,-0.664062" /> + d="m 278.05325,24.906326 l 0,4.535157 l 0.95313,0 c 0.80468,0 1.39322,-0.182291 1.76562,-0.546875 c 0.375,-0.364582 0.5625,-0.940102 0.5625,-1.726563 c 0,-0.781246 -0.1875,-1.35286 -0.5625,-1.714844 c -0.3724,-0.364578 -0.96094,-0.54687 -1.76562,-0.546875 l -0.95313,0 m -0.78906,-0.648437 l 1.62109,0 c 1.13021,6e-6 1.95964,0.235682 2.48829,0.707031 c 0.52864,0.468755 0.79296,1.203129 0.79296,2.203125 c 0,1.00521 -0.26563,1.743491 -0.79687,2.214844 c -0.53125,0.471354 -1.35938,0.707031 -2.48438,0.707031 l -1.62109,0 l 0,-5.832031" /> @@ -1414,34 +1450,34 @@ inkscape:connector-curvature="0" id="path3902" style="font-size:8px;fill:#ffffff" - d="m 283.42044,24.257889 0.78906,0 0,5.832031 -0.78906,0 0,-5.832031" /> + d="m 283.42044,24.257889 l 0.78906,0 l 0,5.832031 l -0.78906,0 l 0,-5.832031" /> + d="m 289.27591,24.449295 l 0,0.769531 c -0.29948,-0.143224 -0.58204,-0.249995 -0.84766,-0.320312 c -0.26562,-0.07031 -0.52214,-0.105464 -0.76953,-0.105469 c -0.42969,5e-6 -0.76172,0.08334 -0.99609,0.25 c -0.23177,0.166672 -0.34766,0.403651 -0.34766,0.710938 c 0,0.257816 0.0768,0.453128 0.23047,0.585937 c 0.15625,0.130212 0.45052,0.235681 0.88281,0.316406 l 0.47657,0.09766 c 0.58853,0.111982 1.02213,0.309898 1.30078,0.59375 c 0.28124,0.281252 0.42187,0.658856 0.42187,1.132812 c 0,0.565105 -0.19011,0.99349 -0.57031,1.285156 c -0.37761,0.291667 -0.9323,0.4375 -1.66406,0.4375 c -0.27605,0 -0.57032,-0.03125 -0.88282,-0.09375 c -0.30989,-0.0625 -0.63151,-0.154947 -0.96484,-0.277343 l 0,-0.8125 c 0.32031,0.179688 0.63411,0.315104 0.94141,0.40625 c 0.30729,0.09115 0.60937,0.136719 0.90625,0.136718 c 0.45051,1e-6 0.79817,-0.08854 1.04297,-0.265625 c 0.24478,-0.177082 0.36718,-0.429686 0.36718,-0.757812 c 0,-0.286457 -0.0885,-0.510415 -0.26562,-0.671875 c -0.17448,-0.161456 -0.46224,-0.28255 -0.86328,-0.363281 l -0.48047,-0.09375 c -0.58855,-0.117185 -1.01433,-0.300779 -1.27735,-0.550782 C 285.65351,26.609458 285.522,26.261802 285.522,25.816486 c 0,-0.515621 0.18099,-0.92187 0.54297,-1.21875 c 0.36458,-0.29687 0.86589,-0.445307 1.50391,-0.445313 c 0.27343,6e-6 0.55208,0.02475 0.83594,0.07422 c 0.28385,0.04949 0.57421,0.123703 0.87109,0.222656" /> @@ -1451,17 +1487,17 @@ id="waypoint-eta-label" transform="matrix(1.0375459,0,0,1.0375459,-14.202279,-4.0166731)"> @@ -1484,27 +1520,27 @@ id="battery-milliamp-text" transform="translate(0,16.75)"> @@ -1524,27 +1560,27 @@ inkscape:connector-curvature="0" id="path4599" style="fill:#ffffff" - d="m 575.65771,9.449295 c -0.50781,6.7e-6 -0.8903,0.2506575 -1.14746,0.751953 -0.2539,0.498052 -0.38086,1.248377 -0.38086,2.250977 0,0.999351 0.12696,1.749676 0.38086,2.250976 0.25716,0.498048 0.63965,0.747071 1.14746,0.747071 0.51107,0 0.89356,-0.249023 1.14747,-0.747071 0.25715,-0.5013 0.38573,-1.251625 0.38574,-2.250976 -10e-6,-1.0026 -0.12859,-1.752925 -0.38574,-2.250977 -0.25391,-0.5012955 -0.6364,-0.7519463 -1.14747,-0.751953 m 0,-0.78125 c 0.81706,7.5e-6 1.44043,0.3239004 1.87012,0.9716797 0.43294,0.6445373 0.64941,1.5820363 0.64942,2.8125003 -10e-6,1.227216 -0.21648,2.164715 -0.64942,2.8125 -0.42969,0.644531 -1.05306,0.966796 -1.87012,0.966797 -0.81705,-10e-7 -1.44205,-0.322266 -1.875,-0.966797 -0.42968,-0.647785 -0.64453,-1.585284 -0.64453,-2.8125 0,-1.230464 0.21485,-2.167963 0.64453,-2.8125003 0.43295,-0.6477793 1.05795,-0.9716722 1.875,-0.9716797" /> + d="m 575.65771,9.449295 c -0.50781,6.7e-6 -0.8903,0.2506575 -1.14746,0.751953 c -0.2539,0.498052 -0.38086,1.248377 -0.38086,2.250977 c 0,0.999351 0.12696,1.749676 0.38086,2.250976 c 0.25716,0.498048 0.63965,0.747071 1.14746,0.747071 c 0.51107,0 0.89356,-0.249023 1.14747,-0.747071 c 0.25715,-0.5013 0.38573,-1.251625 0.38574,-2.250976 c -10e-6,-1.0026 -0.12859,-1.752925 -0.38574,-2.250977 c -0.25391,-0.5012955 -0.6364,-0.7519463 -1.14747,-0.751953 m 0,-0.78125 c 0.81706,7.5e-6 1.44043,0.3239004 1.87012,0.9716797 c 0.43294,0.6445373 0.64941,1.5820363 0.64942,2.8125003 c -10e-6,1.227216 -0.21648,2.164715 -0.64942,2.8125 c -0.42969,0.644531 -1.05306,0.966796 -1.87012,0.966797 c -0.81705,-10e-7 -1.44205,-0.322266 -1.875,-0.966797 c -0.42968,-0.647785 -0.64453,-1.585284 -0.64453,-2.8125 c 0,-1.230464 0.21485,-2.167963 0.64453,-2.8125003 c 0.43295,-0.6477793 1.05795,-0.9716722 1.875,-0.9716797" /> + d="m 582.0249,9.449295 c -0.50781,6.7e-6 -0.8903,0.2506575 -1.14746,0.751953 c -0.25391,0.498052 -0.38086,1.248377 -0.38086,2.250977 c 0,0.999351 0.12695,1.749676 0.38086,2.250976 c 0.25716,0.498048 0.63965,0.747071 1.14746,0.747071 c 0.51107,0 0.89355,-0.249023 1.14746,-0.747071 c 0.25716,-0.5013 0.38574,-1.251625 0.38575,-2.250976 c -1e-5,-1.0026 -0.12859,-1.752925 -0.38575,-2.250977 C 582.91845,9.6999525 582.53597,9.4493017 582.0249,9.449295 m 0,-0.78125 c 0.81706,7.5e-6 1.44043,0.3239004 1.87012,0.9716797 c 0.43294,0.6445373 0.64941,1.5820363 0.64941,2.8125003 c 0,1.227216 -0.21647,2.164715 -0.64941,2.8125 c -0.42969,0.644531 -1.05306,0.966796 -1.87012,0.966797 c -0.81706,-10e-7 -1.44206,-0.322266 -1.875,-0.966797 c -0.42969,-0.647785 -0.64453,-1.585284 -0.64453,-2.8125 c 0,-1.230464 0.21484,-2.167963 0.64453,-2.8125003 c 0.43294,-0.6477793 1.05794,-0.9716722 1.875,-0.9716797" /> + d="m 586.28271,14.849686 l 1.03028,0 l 0,1.240234 l -1.03028,0 l 0,-1.240234" /> + d="m 591.57568,9.449295 c -0.50781,6.7e-6 -0.8903,0.2506575 -1.14746,0.751953 c -0.25391,0.498052 -0.38086,1.248377 -0.38086,2.250977 c 0,0.999351 0.12695,1.749676 0.38086,2.250976 c 0.25716,0.498048 0.63965,0.747071 1.14746,0.747071 c 0.51107,0 0.89355,-0.249023 1.14746,-0.747071 c 0.25716,-0.5013 0.38574,-1.251625 0.38575,-2.250976 c -10e-6,-1.0026 -0.12859,-1.752925 -0.38575,-2.250977 c -0.25391,-0.5012955 -0.63639,-0.7519463 -1.14746,-0.751953 m 0,-0.78125 c 0.81706,7.5e-6 1.44043,0.3239004 1.87012,0.9716797 c 0.43294,0.6445373 0.64941,1.5820363 0.64941,2.8125003 c 0,1.227216 -0.21647,2.164715 -0.64941,2.8125 c -0.42969,0.644531 -1.05306,0.966796 -1.87012,0.966797 c -0.81706,-10e-7 -1.44206,-0.322266 -1.875,-0.966797 c -0.42968,-0.647785 -0.64453,-1.585284 -0.64453,-2.8125 c 0,-1.230464 0.21485,-2.167963 0.64453,-2.8125003 c 0.43294,-0.6477793 1.05794,-0.9716722 1.875,-0.9716797" /> + d="m 597.94287,9.449295 c -0.50781,6.7e-6 -0.8903,0.2506575 -1.14746,0.751953 c -0.25391,0.498052 -0.38086,1.248377 -0.38086,2.250977 c 0,0.999351 0.12695,1.749676 0.38086,2.250976 c 0.25716,0.498048 0.63965,0.747071 1.14746,0.747071 c 0.51107,0 0.89355,-0.249023 1.14746,-0.747071 c 0.25716,-0.5013 0.38574,-1.251625 0.38574,-2.250976 c 0,-1.0026 -0.12858,-1.752925 -0.38574,-2.250977 c -0.25391,-0.5012955 -0.63639,-0.7519463 -1.14746,-0.751953 m 0,-0.78125 c 0.81705,7.5e-6 1.44043,0.3239004 1.87012,0.9716797 c 0.43294,0.6445373 0.64941,1.5820363 0.64941,2.8125003 c 0,1.227216 -0.21647,2.164715 -0.64941,2.8125 c -0.42969,0.644531 -1.05307,0.966796 -1.87012,0.966797 c -0.81706,-10e-7 -1.44206,-0.322266 -1.875,-0.966797 c -0.42969,-0.647785 -0.64453,-1.585284 -0.64453,-2.8125 c 0,-1.230464 0.21484,-2.167963 0.64453,-2.8125003 c 0.43294,-0.6477793 1.05794,-0.9716722 1.875,-0.9716797" /> @@ -1593,7 +1629,7 @@ @@ -1601,13 +1637,13 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="eng1" - d="m 469.86993,48.85 0,-2.5" + d="m 469.86993,48.85 l 0,-2.5" style="fill:#008080;stroke:#008000;stroke-width:8.30000019;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" sodipodi:nodetypes="cc" /> @@ -1615,13 +1651,13 @@ inkscape:label="#path10213" inkscape:connector-curvature="0" id="eng3" - d="m 484.90956,48.85 0,-7.500001" + d="m 484.90956,48.85 l 0,-7.500001" style="fill:none;stroke:#008000;stroke-width:8.30000019;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" sodipodi:nodetypes="cc" /> @@ -1629,13 +1665,13 @@ inkscape:label="#path10217" inkscape:connector-curvature="0" id="eng5" - d="m 499.94918,48.85 0,-12.500001" + d="m 499.94918,48.85 l 0,-12.500001" style="fill:none;stroke:#008000;stroke-width:8.30000019;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" sodipodi:nodetypes="cc" /> @@ -1643,13 +1679,13 @@ inkscape:label="#path10221" inkscape:connector-curvature="0" id="eng7" - d="m 514.98879,48.85 0,-17.500001" + d="m 514.98879,48.85 l 0,-17.500001" style="fill:none;stroke:#008000;stroke-width:8.30000019;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" sodipodi:nodetypes="cc" /> @@ -1657,13 +1693,13 @@ inkscape:label="#path10225" inkscape:connector-curvature="0" id="eng9" - d="m 530.02841,48.85 0,-22.500001" + d="m 530.02841,48.85 l 0,-22.500001" style="fill:none;stroke:#008000;stroke-width:8.30000019;stroke-linecap:square;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" sodipodi:nodetypes="cc" /> @@ -1682,42 +1718,42 @@ id="waypoint-eta-text" transform="translate(0,0.595158)"> @@ -1732,42 +1768,42 @@ id="waypoint-distance-text" transform="translate(0,0.595158)"> @@ -1782,17 +1818,17 @@ id="waypoint-heading-text" transform="translate(2,0.595158)"> @@ -1807,82 +1843,82 @@ id="waypoint-mode-text" transform="translate(0,0.595158)"> @@ -1898,57 +1934,57 @@ id="waypoint-description-text" transform="translate(30,0.595158)"> @@ -1961,7 +1997,7 @@ inkscape:connector-curvature="0" id="path3957" style="font-size:12px;fill:#ff00ff" - d="m 309.28568,10.810623 -1.70508,0 -0.49219,1.957031 1.7168,0 0.48047,-1.957031 m -0.87891,-3.3339842 -0.60938,2.4316406 1.71094,0 0.61524,-2.4316406 0.9375,0 -0.60352,2.4316406 1.82813,0 0,0.9023436 -2.05665,0 -0.48046,1.957031 1.86328,0 0,0.896485 -2.0918,0 -0.60937,2.425781 -0.9375,0 0.60351,-2.425781 -1.7168,0 -0.60351,2.425781 -0.94336,0 0.60937,-2.425781 -1.8457,0 0,-0.896485 2.0625,0 0.49219,-1.957031 -1.88672,0 0,-0.9023436 2.11523,0 0.59766,-2.4316406 0.94922,0" /> + d="m 309.28568,10.810623 l -1.70508,0 l -0.49219,1.957031 l 1.7168,0 l 0.48047,-1.957031 m -0.87891,-3.3339842 l -0.60938,2.4316406 l 1.71094,0 l 0.61524,-2.4316406 l 0.9375,0 l -0.60352,2.4316406 l 1.82813,0 l 0,0.9023436 l -2.05665,0 l -0.48046,1.957031 l 1.86328,0 l 0,0.896485 l -2.0918,0 l -0.60937,2.425781 l -0.9375,0 l 0.60351,-2.425781 l -1.7168,0 l -0.60351,2.425781 l -0.94336,0 l 0.60937,-2.425781 l -1.8457,0 l 0,-0.896485 l 2.0625,0 l 0.49219,-1.957031 l -1.88672,0 l 0,-0.9023436 l 2.11523,0 l 0.59766,-2.4316406 l 0.94922,0" /> + d="m 303.29349,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37696,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.3086,0.597657 0.76758,0.896485 1.37696,0.896484 c 0.61327,1e-6 1.07226,-0.298827 1.37695,-0.896484 c 0.30859,-0.601561 0.46289,-1.50195 0.46289,-2.701172 c 0,-1.20312 -0.1543,-2.103509 -0.46289,-2.701172 c -0.30469,-0.601555 -0.76368,-0.902336 -1.37695,-0.902344 m 0,-0.9375 c 0.98046,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51952,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25978,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26368,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51563,-0.777342 -0.77344,-1.902341 -0.77344,-3.375 c 0,-1.476557 0.25781,-2.601556 0.77344,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 310.93411,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37695,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.30859,0.597657 0.76757,0.896485 1.37695,0.896484 c 0.61328,1e-6 1.07226,-0.298827 1.37696,-0.896484 c 0.30858,-0.601561 0.46288,-1.50195 0.46289,-2.701172 c -10e-6,-1.20312 -0.15431,-2.103509 -0.46289,-2.701172 c -0.3047,-0.601555 -0.76368,-0.902336 -1.37696,-0.902344 m 0,-0.9375 c 0.98047,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51953,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25977,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26367,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51562,-0.777342 -0.77344,-1.902341 -0.77343,-3.375 c -1e-5,-1.476557 0.25781,-2.601556 0.77343,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 318.57474,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37696,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.3086,0.597657 0.76758,0.896485 1.37696,0.896484 c 0.61327,1e-6 1.07226,-0.298827 1.37695,-0.896484 c 0.30859,-0.601561 0.46289,-1.50195 0.46289,-2.701172 c 0,-1.20312 -0.1543,-2.103509 -0.46289,-2.701172 c -0.30469,-0.601555 -0.76368,-0.902336 -1.37695,-0.902344 m 0,-0.9375 c 0.98046,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51952,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25978,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26368,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51563,-0.777342 -0.77344,-1.902341 -0.77344,-3.375 c 0,-1.476557 0.25781,-2.601556 0.77344,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 326.21536,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37695,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.30859,0.597657 0.76757,0.896485 1.37695,0.896484 c 0.61328,1e-6 1.07226,-0.298827 1.37696,-0.896484 c 0.30858,-0.601561 0.46288,-1.50195 0.46289,-2.701172 c -10e-6,-1.20312 -0.15431,-2.103509 -0.46289,-2.701172 c -0.3047,-0.601555 -0.76368,-0.902336 -1.37696,-0.902344 m 0,-0.9375 c 0.98047,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51953,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25977,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26367,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51562,-0.777342 -0.77344,-1.902341 -0.77343,-3.375 c -1e-5,-1.476557 0.25781,-2.601556 0.77343,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 333.85599,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37696,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.3086,0.597657 0.76758,0.896485 1.37696,0.896484 c 0.61327,1e-6 1.07226,-0.298827 1.37695,-0.896484 c 0.30859,-0.601561 0.46289,-1.50195 0.46289,-2.701172 c 0,-1.20312 -0.1543,-2.103509 -0.46289,-2.701172 c -0.30469,-0.601555 -0.76368,-0.902336 -1.37695,-0.902344 m 0,-0.9375 c 0.98046,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51952,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25978,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26368,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51563,-0.777342 -0.77344,-1.902341 -0.77344,-3.375 c 0,-1.476557 0.25781,-2.601556 0.77344,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 341.49661,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37695,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.30859,0.597657 0.76757,0.896485 1.37695,0.896484 c 0.61328,1e-6 1.07226,-0.298827 1.37696,-0.896484 c 0.30858,-0.601561 0.46288,-1.50195 0.46289,-2.701172 c -10e-6,-1.20312 -0.15431,-2.103509 -0.46289,-2.701172 c -0.3047,-0.601555 -0.76368,-0.902336 -1.37696,-0.902344 m 0,-0.9375 c 0.98047,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51953,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25977,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26367,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51562,-0.777342 -0.77344,-1.902341 -0.77343,-3.375 c -1e-5,-1.476557 0.25781,-2.601556 0.77343,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 349.13724,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37696,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.3086,0.597657 0.76758,0.896485 1.37696,0.896484 c 0.61327,1e-6 1.07226,-0.298827 1.37695,-0.896484 c 0.30859,-0.601561 0.46289,-1.50195 0.46289,-2.701172 c 0,-1.20312 -0.1543,-2.103509 -0.46289,-2.701172 c -0.30469,-0.601555 -0.76368,-0.902336 -1.37695,-0.902344 m 0,-0.9375 c 0.98046,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51952,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25978,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26368,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51563,-0.777342 -0.77344,-1.902341 -0.77344,-3.375 c 0,-1.476557 0.25781,-2.601556 0.77344,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> + d="m 356.77786,23.12117 c -0.60938,8e-6 -1.06836,0.300789 -1.37695,0.902344 c -0.30469,0.597663 -0.45703,1.498052 -0.45703,2.701172 c 0,1.199222 0.15234,2.099611 0.45703,2.701172 c 0.30859,0.597657 0.76757,0.896485 1.37695,0.896484 c 0.61328,1e-6 1.07226,-0.298827 1.37696,-0.896484 c 0.30858,-0.601561 0.46288,-1.50195 0.46289,-2.701172 c -10e-6,-1.20312 -0.15431,-2.103509 -0.46289,-2.701172 c -0.3047,-0.601555 -0.76368,-0.902336 -1.37696,-0.902344 m 0,-0.9375 c 0.98047,9e-6 1.72851,0.38868 2.24414,1.166016 c 0.51953,0.773444 0.77929,1.898443 0.7793,3.375 c -1e-5,1.472659 -0.25977,2.597658 -0.7793,3.375 c -0.51563,0.773437 -1.26367,1.160156 -2.24414,1.160156 c -0.98047,0 -1.73047,-0.386719 -2.25,-1.160156 c -0.51562,-0.777342 -0.77344,-1.902341 -0.77343,-3.375 c -1e-5,-1.476557 0.25781,-2.601556 0.77343,-3.375 c 0.51953,-0.777336 1.26953,-1.166007 2.25,-1.166016" /> @@ -2034,35 +2070,35 @@ style="font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Arial;-inkscape-font-specification:Arial" id="telemetry-status"> @@ -2077,77 +2113,77 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx0" - d="m 110,36.5 0,-11.082914" + d="m 110,36.5 l 0,-11.082914" style="fill:none;stroke:#000000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2155,11 +2191,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx13" - d="m 144,36.5 0,-11.082914" + d="m 144,36.5 l 0,-11.082914" style="fill:none;stroke:#008000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2167,11 +2203,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx15" - d="m 149,36.5 0,-11.082914" + d="m 149,36.5 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2179,11 +2215,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx17" - d="m 154,36.5 0,-11.082914" + d="m 154,36.5 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2191,11 +2227,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx19" - d="m 159,36.5 0,-11.082914" + d="m 159,36.5 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2203,11 +2239,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx21" - d="m 164,36.5 0,-11.082914" + d="m 164,36.5 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2215,11 +2251,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx23" - d="m 169,36.5 0,-11.082914" + d="m 169,36.5 l 0,-11.082914" style="fill:none;stroke:#ff1400;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2227,7 +2263,7 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="rx25" - d="m 174,36.5 0,-11.082914" + d="m 174,36.5 l 0,-11.082914" style="fill:none;stroke:#ff1400;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2246,65 +2282,65 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx1" - d="M 114,18.095328 114,7.012414" + d="M 114,18.095328 L 114,7.012414" style="fill:none;stroke:#008000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2312,11 +2348,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx12" - d="m 141.5,18.095328 0,-11.082914" + d="m 141.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#008000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2324,11 +2360,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx14" - d="m 146.5,18.095328 0,-11.082914" + d="m 146.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#008000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2336,11 +2372,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx16" - d="m 151.5,18.095328 0,-11.082914" + d="m 151.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2348,11 +2384,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx18" - d="m 156.5,18.095328 0,-11.082914" + d="m 156.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2360,11 +2396,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx20" - d="m 161.5,18.095328 0,-11.082914" + d="m 161.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#ff8000;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2372,11 +2408,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx22" - d="m 166.5,18.095328 0,-11.082914" + d="m 166.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#ff1400;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2384,11 +2420,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="tx24" - d="m 171.5,18.095328 0,-11.082914" + d="m 171.5,18.095328 l 0,-11.082914" style="fill:none;stroke:#ff1400;stroke-width:3;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;display:inline" /> @@ -2409,11 +2445,11 @@ inkscape:label="#path10180" inkscape:connector-curvature="0" id="gps0" - d="m 28,51.5 0,-5" + d="m 28,51.5 l 0,-5" style="fill:none;stroke:#000000;stroke-width:4;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;display:inline" /> @@ -2493,12 +2529,12 @@ id="gps-mode-text" transform="translate(0,1.5)"> @@ -2517,68 +2553,68 @@ id="battery-milliamp-label" transform="matrix(1,0,0,1.0375514,-2,-2.6776728)"> @@ -2591,7 +2627,7 @@ style="display:inline"> + inkscape:groupmode="layer"> @@ -4372,10 +4407,9 @@ inkscape:groupmode="layer" id="layer32" inkscape:label="altitude-window" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> @@ -4543,7 +4577,7 @@ inkscape:connector-curvature="0" id="path10043" transform="translate(0,4)" - d="m 506.5,144.4375 0,12.75 -9.525,6.5625 0,2.5 9.525,6.5625 0,12.75 43,0 0,-20.5625 0,-20.5625 z" + d="m 506.5,144.4375 l 0,12.75 l -9.525,6.5625 l 0,2.5 l 9.525,6.5625 l 0,12.75 l 43,0 L 549.5,165 l 0,-20.5625 z" style="fill:#000000;stroke:#ffffff" /> + style="display:inline"> @@ -4610,164 +4643,295 @@ inkscape:groupmode="layer" id="layer38" inkscape:label="vsi" - style="display:inline" - sodipodi:insensitive="true"> - - - - - - - - + style="display:inline"> - + - - - - - - - - - - - - - - - - - - - - - + inkscape:connector-curvature="0" + sodipodi:nodetypes="zczzz" /> + style="display:inline" /> + + + + + + + + + + + sodipodi:nodetypes="ccccc" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Date: Tue, 19 Aug 2014 14:16:48 +0200 Subject: [PATCH 163/718] OP-1438 VSI_Arc_PFD+Status : Added FlightTime, ArmStatus, Stab info --- .../openpilotgcs/pfd/default/Warnings.qml | 114 ++++++++- .../share/openpilotgcs/pfd/default/pfd.svg | 240 +++++++++++++----- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 4 +- 3 files changed, 293 insertions(+), 65 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 82030e16b..701183d19 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -6,6 +6,30 @@ Item { // Uninitialised, OK, Warning, Error, Critical property variant statusColors : ["gray", "green", "red", "red", "red"] + // DisArmed , Arming, Armed + property variant armColors : ["gray", "orange", "green"] + + // All 'manual modes' are green, 'assisted' modes in cyan + // "MANUAL","STAB 1","STAB 2", "STAB 3", "STAB 4", "STAB 5", "STAB 6", "AUTOTUNE", + // "POS HOLD", "POS VFPV", "POS VLOS", "POS VNSEW", "RTB", "LAND", "PATHPLANNER", "POI", "AUTOCRUISE" + + property variant flightmodeColors : ["gray", "green", "green", "green", "green", "green", "green", "red", + "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan"] + + property real flight_time: Math.round(SystemStats.FlightTime / 1000) + property real time_h: (flight_time > 0 ? Math.floor(flight_time / 3600) : 0 ) + property real time_m: (flight_time > 0 ? Math.floor((flight_time - time_h*3600)/60) : 0) + property real time_s: (flight_time > 0 ? Math.floor(flight_time - time_h*3600 - time_m*60) : 0) + + function formatTime(time) { + if (time === 0) + return "00" + if (time < 10) + return "0" + time; + else + return time.toString(); + } + SvgElementImage { id: warning_bg elementName: "warnings-bg" @@ -14,10 +38,64 @@ Item { anchors.bottom: parent.bottom } + SvgElementPositionItem { + id: warning_time + sceneSize: parent.sceneSize + elementName: "warning-time" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height + + Rectangle { + anchors.fill: parent + color: (SystemStats.FlightTime > 0 ? "green" : "grey") + + Text { + anchors.centerIn: parent + text: formatTime(time_h) + ":" + formatTime(time_m) + ":" + formatTime(time_s) + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 0.8) + weight: Font.DemiBold + } + } + } + } + + SvgElementPositionItem { + id: warning_arm + sceneSize: parent.sceneSize + elementName: "warning-arm" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height + + Rectangle { + anchors.fill: parent + color: warnings.armColors[FlightStatus.Armed] + + Text { + anchors.centerIn: parent + text: ["DISARMED","ARMING","ARMED"][FlightStatus.Armed] + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 0.8) + weight: Font.DemiBold + } + } + } + } + SvgElementPositionItem { id: warning_rc_input sceneSize: parent.sceneSize elementName: "warning-rc-input" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height Rectangle { anchors.fill: parent @@ -39,6 +117,10 @@ Item { id: warning_master_caution sceneSize: parent.sceneSize elementName: "warning-master-caution" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height property bool warningActive: (SystemAlarms.Alarm_BootFault > 1 || SystemAlarms.Alarm_OutOfMemory > 1 || @@ -48,7 +130,7 @@ Item { Rectangle { anchors.fill: parent color: parent.warningActive ? "red" : "red" - opacity: parent.warningActive ? 1.0 : 0.15 + opacity: parent.warningActive ? 1.0 : 0.4 Text { anchors.centerIn: parent @@ -66,6 +148,10 @@ Item { id: warning_autopilot sceneSize: parent.sceneSize elementName: "warning-autopilot" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height Rectangle { anchors.fill: parent @@ -83,6 +169,32 @@ Item { } } + SvgElementPositionItem { + id: warning_flightmode + sceneSize: parent.sceneSize + elementName: "warning-flightmode" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height + + Rectangle { + anchors.fill: parent + color: warnings.flightmodeColors[FlightStatus.FlightMode] + + Text { + anchors.centerIn: parent + text: ["MANUAL","STAB 1","STAB 2", "STAB 3", "STAB 4", "STAB 5", "STAB 6", "AUTOTUNE", "POS HOLD", "POS VFPV", + "POS VLOS", "POS VNSEW", "RTB", "LAND", "PATHPLAN", "POI", "AUTOCRUISE"][FlightStatus.FlightMode] + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 0.8) + weight: Font.DemiBold + } + } + } + } + SvgElementImage { id: warning_gps elementName: "warning-gps" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index 999e32c03..d8b2dd653 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -85,10 +85,10 @@ inkscape:pageopacity="0.0" inkscape:pageshadow="2" inkscape:zoom="1" - inkscape:cx="321.52993" - inkscape:cy="103.08754" + inkscape:cx="313.78803" + inkscape:cy="164.34379" inkscape:document-units="px" - inkscape:current-layer="layer69" + inkscape:current-layer="layer29" showgrid="true" fit-margin-top="0" fit-margin-left="0" @@ -4643,12 +4643,14 @@ inkscape:groupmode="layer" id="layer38" inkscape:label="vsi" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true" /> + style="display:inline" + sodipodi:insensitive="true" /> @@ -4696,12 +4700,14 @@ inkscape:groupmode="layer" id="layer40" inkscape:label="vsi-bar" - style="display:inline" /> + style="display:inline" + sodipodi:insensitive="true" /> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> @@ -4938,8 +4945,7 @@ inkscape:groupmode="layer" id="layer67" inkscape:label="warnings" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> @@ -4965,11 +4971,11 @@ inkscape:groupmode="layer" id="layer51" inkscape:label="warning-autopilot" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> + inkscape:label="#g4860" + transform="translate(1,0)"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + 00.00.00 + + + style="display:inline"> @@ -5052,67 +5165,67 @@ inkscape:label="#warning-master-caution-label" transform="translate(-0.37041,0.11598)"> @@ -5123,11 +5236,11 @@ inkscape:groupmode="layer" id="layer33" inkscape:label="warning-rc-input" - style="display:inline" - sodipodi:insensitive="true"> + style="display:inline"> + inkscape:label="#g4855" + transform="translate(-0.5,0)"> @@ -5200,11 +5313,11 @@ sodipodi:nodetypes="cc" inkscape:connector-curvature="0" id="path4819" - d="M 263.58049,88.871256 374.85741,260.61166" + d="M 263.58049,88.871256 L 374.85741,260.61166" style="fill:none;stroke:#ff0000;stroke-width:6;stroke-linecap:round;stroke-linejoin:miter;stroke-opacity:1" /> @@ -5243,11 +5356,11 @@ sodipodi:nodetypes="cc" inkscape:connector-curvature="0" id="path4404" - d="M 2.1088442,2.592752 86.716324,34.298858" + d="M 2.1088442,2.592752 L 86.716324,34.298858" style="fill:none;stroke:#ff0000;stroke-width:3.00662947;stroke-linecap:round;stroke-linejoin:miter;stroke-opacity:1" /> @@ -5273,7 +5386,7 @@ y="1.2" /> @@ -5281,7 +5394,7 @@ sodipodi:nodetypes="cc" inkscape:connector-curvature="0" id="path4611" - d="M 85.494821,2.8500377 2.2773164,34.130511" + d="M 85.494821,2.8500377 L 2.2773164,34.130511" style="fill:none;stroke:#ff0000;stroke-width:3.00662947;stroke-linecap:round;stroke-linejoin:miter;stroke-opacity:1" /> @@ -5307,11 +5420,11 @@ sodipodi:nodetypes="cc" inkscape:connector-curvature="0" id="path4309" - d="M 2.3796722,2.8075235 86.665165,34.123438" + d="M 2.3796722,2.8075235 L 86.665165,34.123438" style="fill:none;stroke:#ff0000;stroke-width:3.00634193;stroke-linecap:round;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> @@ -5322,9 +5435,10 @@ inkscape:groupmode="layer" id="layer69" inkscape:label="foreground_layer" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> getObject(); From 47aadd25cd2d0f4a8acf3d44d4ec5c2ff8a96ba3 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 19 Aug 2014 16:26:05 +0200 Subject: [PATCH 164/718] OP-1438 VSI_Arc_PFD+Status : Vertical velocity display looks ugly due to telemetry updates, add some filtering. --- .../openpilotgcs/pfd/default/VsiScale.qml | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml index 3f0f901a2..3855ef765 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/VsiScale.qml @@ -3,6 +3,14 @@ import QtQuick 2.0 Item { id: sceneItem property variant sceneSize + property real vert_velocity + + Timer { + interval: 100; running: true; repeat: true + onTriggered: vert_velocity = (0.9 * vert_velocity) + (0.1 * VelocityState.Down) + } + + SvgElementImage { id: vsi_window @@ -31,9 +39,9 @@ Item { //rotate it around the center transform: Rotation { - angle: -VelocityDesired.Down*5 - origin.y : vsi_waypoint.height/2 - origin.x : vsi_waypoint.width*33 + angle: -VelocityDesired.Down * 5 + origin.y : vsi_waypoint.height / 2 + origin.x : vsi_waypoint.width * 33 } } @@ -63,9 +71,9 @@ Item { //rotate it around the center transform: Rotation { - angle: -VelocityState.Down*5 - origin.y : vsi_arrow.height/2 - origin.x : vsi_arrow.width*3.15 + angle: -vert_velocity * 5 + origin.y : vsi_arrow.height / 2 + origin.x : vsi_arrow.width * 3.15 } } From 2c25d2d875e8491c919ebd22ab35c3f1234b1861 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Tue, 19 Aug 2014 20:47:21 +0200 Subject: [PATCH 165/718] OP-1438 VSI_Arc_PFD+Status : Added Thrust mode info close to Flightmode --- .../openpilotgcs/pfd/default/Warnings.qml | 42 ++ .../share/openpilotgcs/pfd/default/pfd.svg | 573 ++++++++++-------- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 5 +- 3 files changed, 362 insertions(+), 258 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index 701183d19..b852fe1a4 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -16,6 +16,19 @@ Item { property variant flightmodeColors : ["gray", "green", "green", "green", "green", "green", "green", "red", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan", "cyan"] + // Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude, + // AltitudeHold,AltitudeVario,CruiseControl + Auto mode (VTOL/Wing pathfollower) + // grey : 'disabled' modes + + property variant thrustmodeColors : ["green", "grey", "grey", "grey", "grey", "grey", "grey", "grey", "grey", + "green", "green", "green", "cyan"] + + property var thrust_mode: FlightStatus.FlightMode < 7 ? StabilizationDesired.StabilizationMode_Thrust : + FlightStatus.FlightMode > 7 && HwSettings.OptionalModules_VtolPathFollower == 1 + && VtolPathFollowerSettings.ThrustControl == 1 ? 12 : + FlightStatus.FlightMode > 7 && HwSettings.OptionalModules_FixedWingPathFollower == 1 ? 12: 0 + + property real flight_time: Math.round(SystemStats.FlightTime / 1000) property real time_h: (flight_time > 0 ? Math.floor(flight_time / 3600) : 0 ) property real time_m: (flight_time > 0 ? Math.floor((flight_time - time_h*3600)/60) : 0) @@ -195,6 +208,35 @@ Item { } } + SvgElementPositionItem { + id: warning_thrustmode + sceneSize: parent.sceneSize + elementName: "warning-thrustmode" + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + x: scaledBounds.x * sceneItem.width + y: scaledBounds.y * sceneItem.height + + Rectangle { + anchors.fill: parent + color: warnings.thrustmodeColors[thrust_mode.toString()] + + // Manual,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar,Rattitude,RelayRate,RelayAttitude, + // AltitudeHold,AltitudeVario,CruiseControl + // grey : 'disabled' modes + Text { + anchors.centerIn: parent + text: ["MANUAL"," "," ", " ", " ", " ", " ", " ", " ", + "ALT HOLD", "ALT VARIO", "CRUIZECTRL", "AUTO"][thrust_mode.toString()] + font { + family: "Arial" + pixelSize: Math.floor(parent.height * 0.8) + weight: Font.DemiBold + } + } + } + } + SvgElementImage { id: warning_gps elementName: "warning-gps" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index d8b2dd653..2b83893a0 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -84,11 +84,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="1" - inkscape:cx="313.78803" - inkscape:cy="164.34379" + inkscape:zoom="9.3372094" + inkscape:cx="596.49986" + inkscape:cy="8" inkscape:document-units="px" - inkscape:current-layer="layer29" + inkscape:current-layer="g3935" showgrid="true" fit-margin-top="0" fit-margin-left="0" @@ -150,7 +150,7 @@ image/svg+xml - + @@ -193,6 +193,20 @@ style="fill:none;stroke:#ffffff;stroke-width:6;stroke-linecap:round;stroke-linejoin:miter;stroke-opacity:1;display:inline" /> + + + @@ -254,37 +268,37 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label10R"> @@ -293,36 +307,36 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label20R"> @@ -331,12 +345,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-20R"> @@ -344,23 +358,23 @@ @@ -369,37 +383,37 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-10R"> @@ -408,12 +422,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label30R"> @@ -421,11 +435,11 @@ @@ -447,12 +461,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label40R"> @@ -460,11 +474,11 @@ @@ -486,37 +500,37 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label50R"> @@ -525,37 +539,37 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label60R"> @@ -564,12 +578,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label70R"> @@ -577,23 +591,23 @@ @@ -602,25 +616,25 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label80R"> @@ -642,12 +656,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label90R"> @@ -655,23 +669,23 @@ @@ -680,36 +694,36 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-40R"> @@ -718,36 +732,36 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-30R"> @@ -756,12 +770,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-60R"> @@ -769,23 +783,23 @@ @@ -794,12 +808,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-50R"> @@ -807,23 +821,23 @@ @@ -832,36 +846,36 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-80R"> @@ -870,24 +884,24 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-70R"> @@ -895,12 +909,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-90L"> @@ -909,12 +923,12 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans" id="pitch_label-90R"> @@ -2678,7 +2692,7 @@ transform="translate(0,4)" inkscape:transform-center-x="31.416405" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" - d="M 290.26594,60.402428 286.90125,47.845226" + d="M 290.26594,60.402428 L 286.90125,47.845226" id="path3442" inkscape:connector-curvature="0" inkscape:transform-center-y="-117.24761" /> @@ -2687,14 +2701,14 @@ inkscape:transform-center-y="-105.12133" inkscape:connector-curvature="0" id="path3444" - d="m 262.55821,71.879346 -6.50008,-11.25848" + d="m 262.55821,71.879346 l -6.50008,-11.25848" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="60.69183" /> @@ -2713,7 +2727,7 @@ sodipodi:nodetypes="cc" inkscape:transform-center-x="20.198055" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" - d="M 300.02868,57.474202 298.9979,52.82864" + d="M 300.02868,57.474202 L 298.9979,52.82864" id="path3464" inkscape:connector-curvature="0" inkscape:transform-center-y="-109.4812" /> @@ -2723,7 +2737,7 @@ inkscape:transform-center-y="-104.31057" inkscape:connector-curvature="0" id="path3466" - d="m 281.77925,62.464332 -1.94063,-4.28458" + d="m 281.77925,62.464332 l -1.94063,-4.28458" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="38.90241" /> @@ -2740,14 +2754,14 @@ inkscape:transform-center-y="-117.24761" inkscape:connector-curvature="0" id="path3491" - d="m 349.73406,60.402428 3.36469,-12.557202" + d="m 349.73406,60.402428 l 3.36469,-12.557202" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="31.416405" /> @@ -2756,7 +2770,7 @@ inkscape:transform-center-y="-85.831206" inkscape:connector-curvature="0" id="path3495" - d="m 399.82423,91.345686 11.40936,-9.763523" + d="m 399.82423,91.345686 l 11.40936,-9.763523" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="85.83121" sodipodi:nodetypes="cc" /> @@ -2765,7 +2779,7 @@ inkscape:transform-center-y="-51.353944" inkscape:connector-curvature="0" id="path3497" - d="m 330.38014,56.242757 0.76566,-4.817725" + d="m 330.38014,56.242757 l 0.76566,-4.817725" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="10.57928" sodipodi:nodetypes="cc" /> @@ -2774,7 +2788,7 @@ inkscape:transform-center-y="-109.4812" inkscape:connector-curvature="0" id="path3499" - d="M 339.97132,57.474202 341.0021,52.82864" + d="M 339.97132,57.474202 L 341.0021,52.82864" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="20.198055" sodipodi:nodetypes="cc" /> @@ -2782,7 +2796,7 @@ transform="translate(0,4)" inkscape:transform-center-x="38.90241" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" - d="m 358.22075,62.464332 1.94063,-4.28458" + d="m 358.22075,62.464332 l 1.94063,-4.28458" id="path3501" inkscape:connector-curvature="0" inkscape:transform-center-y="-104.31057" @@ -2792,7 +2806,7 @@ inkscape:transform-center-y="-100.52307" inkscape:connector-curvature="0" id="path3503" - d="m 367.00252,66.39497 2.31603,-4.30844" + d="m 367.00252,66.39497 l 2.31603,-4.30844" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" inkscape:transform-center-x="47.84564" sodipodi:nodetypes="cc" /> @@ -2801,7 +2815,7 @@ sodipodi:open="true" sodipodi:end="5.5477076" sodipodi:start="3.8773293" - d="M 238.84479,91.530033 C 279.42114,46.70925 348.64918,43.268451 393.46997,83.844794 c 2.69442,2.439265 5.26569,5.011203 7.70426,7.706256" + d="m 238.84479,91.530033 a 109.47147,109.47147 0 0 1 162.32944,0.02102" sodipodi:ry="109.47147" sodipodi:rx="109.47147" sodipodi:cy="165" @@ -2811,7 +2825,7 @@ sodipodi:type="arc" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - 00.00.00 - - + + + + + + + + + + + + + + 00.00.00 + + - - - getObject(); From 779eb8d7729c0b575e965a41989e0f571fc8b0be Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 19 Aug 2014 23:14:46 +0200 Subject: [PATCH 166/718] OP-1156 placed Werners positionhold speed offset back in place --- flight/libraries/plans.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 0faac2a2a..2d009f9ea 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -127,12 +127,15 @@ void plan_setup_returnToBase() static PiOSDeltatimeConfig landdT; void plan_setup_land() { + float descendspeed; + plan_setup_positionHold(); + FlightModeSettingsLandingVelocityGet(&descendspeed); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.StartingVelocity = 0.0f; - pathDesired.EndingVelocity = 0.0f; + pathDesired.StartingVelocity = descendspeed; + pathDesired.EndingVelocity = descendspeed; PathDesiredSet(&pathDesired); PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); } From 8d0f4e71ea7c8f64d83eeb97a074327255633889 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 20 Aug 2014 02:10:31 +0200 Subject: [PATCH 167/718] OP-1438 VSI_Arc_PFD+Status : Added RcInput panel, work in progress --- .../share/openpilotgcs/pfd/default/Info.qml | 102 ++++ .../share/openpilotgcs/pfd/default/pfd.svg | 483 ++++++++++++++---- .../src/plugins/pfdqml/pfdqmlgadgetwidget.cpp | 3 +- 3 files changed, 501 insertions(+), 87 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 449389ca1..9f5a3ce2a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -36,6 +36,8 @@ Item { property real total_distance property bool init_dist: false + property bool hide_display_rc: false + function reset_distance(){ total_distance = 0; } @@ -59,6 +61,14 @@ Item { else return time.toString(); } + + function hide_display_rcinput(){ + console.log("module: "+hide_display_rc); + if (hide_display_rc == false) + hide_display_rc = true; + else + hide_display_rc = false; + } SvgElementImage { id: info_bg @@ -462,6 +472,98 @@ Item { } + SvgElementImage { + id: rc_input_bg + elementName: "rc-input-bg" + sceneSize: info.sceneSize + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading" + when: hide_display_rc !== true + PropertyChanges { target: rc_input_bg; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + } + + SvgElementImage { + id: rc_input_labels + elementName: "rc-input-labels" + sceneSize: info.sceneSize + y: Math.floor(scaledBounds.y * sceneItem.height) + + states: State { + name: "fading" + when: hide_display_rc !== true + PropertyChanges { target: rc_input_labels; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + } + + SvgElementImage { + id: rc_input_icon + elementName: "rc-input-icon" + sceneSize: info.sceneSize + y: Math.floor(scaledBounds.y * sceneItem.height) + + MouseArea { id: hidedisp_rcinput; anchors.fill: parent; onClicked: hide_display_rcinput()} + + states: State { + name: "fading" + when: hide_display_rc !== true + PropertyChanges { target: rc_input_icon; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + } + + SvgElementImage { + id: rc_stick + elementName: "rc-stick" + sceneSize: info.sceneSize + + width: scaledBounds.width * sceneItem.width + height: scaledBounds.height * sceneItem.height + + x: (scaledBounds.x * sceneItem.width) + (ManualControlCommand.Roll * rc_stick.width *2.5) + y: (scaledBounds.y * sceneItem.height) + (ManualControlCommand.Pitch * rc_stick.width * 2.5) + + smooth: true + + //rotate it around the center of horizon + transform: Rotation { + angle: ManualControlCommand.Yaw * 90 + origin.y : rc_stick.height / 2 + origin.x : rc_stick.width / 2 + } + + states: State { + name: "fading" + when: hide_display_rc !== true + PropertyChanges { target: rc_stick; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + } + + transitions: Transition { + SequentialAnimation { + PropertyAnimation { property: "x"; duration: 800 } + } + } + } + SvgElementImage { id: info_border elementName: "info-border" diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index 2b83893a0..c59921c0a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -2,6 +2,7 @@ + + + + + + + + + + + + + + + + + @@ -963,17 +1057,17 @@ id="home-eta-label" transform="matrix(1,0,0,1.0973877,0,-46.442937)"> @@ -983,22 +1077,22 @@ id="home-distance-label" transform="matrix(1,0,0,1.0577142,0,-27.456636)"> @@ -1007,29 +1101,29 @@ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;display:inline;font-family:Sans" id="home-label"> @@ -1065,42 +1159,42 @@ id="home-eta-text" transform="matrix(1,0,0,0.99160769,0,2.0646588)"> @@ -1117,37 +1211,37 @@ id="home-distance-text" transform="matrix(1,0,0,0.99160769,0,2.0975587)"> @@ -1164,23 +1258,229 @@ id="home-heading-text" transform="matrix(1,0,0,0.99160769,15.28151,1.9884587)"> + + + + + + + + + + + + + + + + + + + + + + PITCH + ROLL + + + + + + + + style="display:inline" + sodipodi:insensitive="true"> @@ -4203,104 +4504,104 @@ @@ -4308,12 +4609,12 @@ inkscape:label="#path3532" inkscape:connector-curvature="0" id="speed5" - d="M 65.000001,-31 52,-31" + d="M 65.000001,-31 L 52,-31" style="fill:none;stroke:#ffffff;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" /> @@ -4332,7 +4633,7 @@ sodipodi:nodetypes="cccccccccc" inkscape:connector-curvature="0" id="path10059" - d="m 91,148.5 0,20.5 0,20.5 44,0 0,-9.78125 8.5,-9.46875 0,-2.5 -8.5,-9.46875 0,-9.78125 z" + d="m 91,148.5 l 0,20.5 l 0,20.5 l 44,0 l 0,-9.78125 l 8.5,-9.46875 l 0,-2.5 L 135,158.28125 L 135,148.5 z" style="fill:#000000;stroke:#ffffff" /> @@ -4367,27 +4668,27 @@ id="speed-text" transform="translate(0,42)"> @@ -4399,7 +4700,8 @@ style="display:inline" inkscape:label="altitude" id="g9762" - inkscape:groupmode="layer"> + inkscape:groupmode="layer" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> + inkscape:label="warning-thrustmode" + sodipodi:insensitive="true"> + inkscape:label="warning-flightmode" + sodipodi:insensitive="true"> @@ -5090,7 +5397,8 @@ inkscape:groupmode="layer" id="layer51" inkscape:label="warning-autopilot" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> + style="display:inline" + sodipodi:insensitive="true"> @@ -5247,7 +5556,8 @@ inkscape:groupmode="layer" id="layer33" inkscape:label="warning-rc-input" - style="display:inline"> + style="display:inline" + sodipodi:insensitive="true"> + inkscape:label="warning-arm" + sodipodi:insensitive="true"> diff --git a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp index 1055a71c8..ad8cb4a27 100644 --- a/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfdqml/pfdqmlgadgetwidget.cpp @@ -72,7 +72,8 @@ PfdQmlGadgetWidget::PfdQmlGadgetWidget(QWindow *parent) : "SystemStats" << "StabilizationDesired" << "VtolPathFollowerSettings" << - "HwSettings"; + "HwSettings" << + "ManualControlCommand"; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); From 8447f474fd666d6eab3843cf23531481db2e9cf7 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 20 Aug 2014 13:32:19 +0200 Subject: [PATCH 168/718] OP-1438 VSI_Arc_PFD+Status : Typo (CruiseCtrl), little cleanup --- .../share/openpilotgcs/pfd/default/Info.qml | 30 +++++++++---------- .../openpilotgcs/pfd/default/Warnings.qml | 2 +- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 9f5a3ce2a..7f8270a98 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -63,11 +63,10 @@ Item { } function hide_display_rcinput(){ - console.log("module: "+hide_display_rc); - if (hide_display_rc == false) - hide_display_rc = true; - else - hide_display_rc = false; + if (hide_display_rc == false) + hide_display_rc = true; + else + hide_display_rc = false; } SvgElementImage { @@ -92,7 +91,6 @@ Item { } } - Repeater { id: satNumberBar @@ -369,7 +367,7 @@ Item { states: State { name: "fading" when: TakeOffLocation.Status !== 0 - PropertyChanges { target: home_bg; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + PropertyChanges { target: home_bg; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } } transitions: Transition { @@ -390,7 +388,7 @@ Item { states: State { name: "fading_heading" when: TakeOffLocation.Status !== 0 - PropertyChanges { target: home_heading_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width } + PropertyChanges { target: home_heading_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } } transitions: Transition { @@ -420,7 +418,7 @@ Item { states: State { name: "fading_distance" when: TakeOffLocation.Status !== 0 - PropertyChanges { target: home_distance_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + PropertyChanges { target: home_distance_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } } transitions: Transition { @@ -451,7 +449,7 @@ Item { states: State { name: "fading_distance" when: TakeOffLocation.Status !== 0 - PropertyChanges { target: home_eta_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } + PropertyChanges { target: home_eta_text; x: Math.floor(scaledBounds.x * sceneItem.width) + home_bg.width; } } transitions: Transition { @@ -481,7 +479,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_bg; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_bg; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } } transitions: Transition { @@ -500,7 +498,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_labels; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_labels; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } } transitions: Transition { @@ -521,7 +519,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_icon; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_icon; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } } transitions: Transition { @@ -539,12 +537,12 @@ Item { width: scaledBounds.width * sceneItem.width height: scaledBounds.height * sceneItem.height - x: (scaledBounds.x * sceneItem.width) + (ManualControlCommand.Roll * rc_stick.width *2.5) + x: (scaledBounds.x * sceneItem.width) + (ManualControlCommand.Roll * rc_stick.width * 2.5) y: (scaledBounds.y * sceneItem.height) + (ManualControlCommand.Pitch * rc_stick.width * 2.5) smooth: true - //rotate it around the center of horizon + //rotate it around his center transform: Rotation { angle: ManualControlCommand.Yaw * 90 origin.y : rc_stick.height / 2 @@ -554,7 +552,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_stick; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_stick; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } } transitions: Transition { diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml index b852fe1a4..8d1ef4a36 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Warnings.qml @@ -227,7 +227,7 @@ Item { Text { anchors.centerIn: parent text: ["MANUAL"," "," ", " ", " ", " ", " ", " ", " ", - "ALT HOLD", "ALT VARIO", "CRUIZECTRL", "AUTO"][thrust_mode.toString()] + "ALT HOLD", "ALT VARIO", "CRUISECTRL", "AUTO"][thrust_mode.toString()] font { family: "Arial" pixelSize: Math.floor(parent.height * 0.8) From a00de4aaaecaae4d6ac98cfcd67014bec3a9429d Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 20 Aug 2014 13:36:05 +0200 Subject: [PATCH 169/718] OP-1438 VSI_Arc_PFD+Status : Bigger Rcinput panel for best viewing --- .../share/openpilotgcs/pfd/default/Info.qml | 8 +- .../share/openpilotgcs/pfd/default/pfd.svg | 191 +++++++++++------- 2 files changed, 117 insertions(+), 82 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml index 7f8270a98..c6cc2fa5f 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml @@ -479,7 +479,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_bg; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_bg; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.85); } } transitions: Transition { @@ -498,7 +498,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_labels; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_labels; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.85); } } transitions: Transition { @@ -519,7 +519,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_input_icon; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_input_icon; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.85); } } transitions: Transition { @@ -552,7 +552,7 @@ Item { states: State { name: "fading" when: hide_display_rc !== true - PropertyChanges { target: rc_stick; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.91); } + PropertyChanges { target: rc_stick; x: Math.floor(scaledBounds.x * sceneItem.width) - (rc_input_bg.width * 0.85); } } transitions: Transition { diff --git a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg index c59921c0a..2fb698ff8 100644 --- a/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg +++ b/ground/openpilotgcs/share/openpilotgcs/pfd/default/pfd.svg @@ -22,6 +22,17 @@ inkscape:export-ydpi="72"> + + + + @@ -139,13 +150,13 @@ id="linearGradient5006-2" xlink:href="#linearGradient4983-7" inkscape:collect="always" - gradientTransform="matrix(1.0909091,0,0,1,-7.2727273,0)" /> + gradientTransform="matrix(1.2653937,0,0,1.1601679,-8.5036667,-73.769595)" /> + + id="rc-input-bg" + transform="matrix(1.1599442,0,0,1.1601679,-0.06770889,-73.769595)"> + d="m -1.8968835,311 c -2.77,0 -5,2.23 -5,5 l 0,140 c 0,2.77 2.23,5 5,5 L 145,461 l 19,0 c 2.77,0 5,-2.23 5,-5 l 0,-30 c 0,-2.77 -2.23,-5 -5,-5 l -14,0 l 0,-105 c 0,-2.77 -2.23,-5 -5,-5 z" + style="fill:#2c2929;fill-opacity:1;stroke:#ffffff;stroke-width:0.86202729;stroke-opacity:1" + sodipodi:nodetypes="sssscsssscsss" /> + style="fill:none;stroke:#ffffff;stroke-width:0.86202729;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> + transform="matrix(1.7211673,0,0,1.526957,-84.861643,-237.54055)"> + style="fill:#464646;fill-opacity:1;stroke:#ffffff;stroke-width:0.43179047;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" /> + transform="matrix(0.25,0,0,0.25,114.18623,329.53888)" /> + transform="matrix(-0.625,0,0,-0.625,253.625,715.23272)" /> + width="4.352685" + height="2.0068226" + x="152.5251" + y="442.32941" + rx="0.29050052" + ry="0.32744864" /> + d="m 154.54064,436.85572 l 0,-1.69713 l 5,-3" + style="fill:none;stroke:#ffffff;stroke-width:0.80189669;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-start:none" /> + + d="m 140,385 a 65,65 0 1 1 -130,0 a 65,65 0 1 1 130,0 z" + transform="matrix(1.1599442,0,0,1.1601679,-0.06770889,-73.769595)" /> + width="69.610069" + height="11.599442" + x="-367.09421" + y="81.12838" /> PITCH ROLL + width="69.596649" + height="11.601679" + x="-81.12838" + y="-378.69589" /> @@ -1447,31 +1480,33 @@ sodipodi:cy="385" sodipodi:cx="75" id="path5029" - style="fill:#1e1e1e;fill-opacity:1;stroke:#5555e6;stroke-width:4;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;fill-rule:nonzero" - sodipodi:type="arc" /> + style="fill:#1e1e1e;fill-opacity:1;fill-rule:nonzero;stroke:#5555e6;stroke-width:3.44810915;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none" + sodipodi:type="arc" + transform="matrix(1.1599442,0,0,1.1601679,-0.06770889,-73.769595)" />