From 094a1e4e12f34c1ffc0d1e2d584c7599daa481db Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 9 Jun 2013 17:28:28 +0200 Subject: [PATCH 01/11] OP-996 Added GCS option to remember and restore the last selected workspace Also reorganized the workspace options page --- .../plugins/coreplugin/connectionmanager.cpp | 4 +- .../plugins/coreplugin/connectionmanager.h | 14 +- .../src/plugins/coreplugin/mainwindow.cpp | 16 ++- .../coreplugin/uavgadgetinstancemanager.cpp | 4 + .../plugins/coreplugin/workspacesettings.cpp | 17 ++- .../plugins/coreplugin/workspacesettings.h | 5 + .../plugins/coreplugin/workspacesettings.ui | 123 ++++++++++-------- 7 files changed, 110 insertions(+), 73 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index db684e4c1..c12919a14 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -40,7 +40,7 @@ #include namespace Core { -ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) : +ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow) : QWidget(mainWindow), m_availableDevList(0), m_connectBtn(0), @@ -73,8 +73,6 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge setLayout(layout); - modeStack->setCornerWidget(this, Qt::TopRightCorner); - QObject::connect(m_connectBtn, SIGNAL(clicked()), this, SLOT(onConnectClicked())); QObject::connect(m_availableDevList, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelectionChanged(int))); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index 71eee425f..1f339818a 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -29,11 +29,11 @@ #ifndef CONNECTIONMANAGER_H #define CONNECTIONMANAGER_H -#include #include "mainwindow.h" #include "generalsettings.h" #include "telemetrymonitorwidget.h" #include +#include #include #include #include @@ -43,20 +43,14 @@ #include "core_global.h" #include -QT_BEGIN_NAMESPACE -class QTabWidget; -QT_END_NAMESPACE - namespace Core { + class IConnection; namespace Internal { -class FancyTabWidget; -class FancyActionBar; -class MainWindow; + class MainWindow; } // namespace Internal - class DevListItem { public: DevListItem(IConnection *c, IConnection::device d) : @@ -86,7 +80,7 @@ class CORE_EXPORT ConnectionManager : public QWidget { Q_OBJECT public: - ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack); + ConnectionManager(Internal::MainWindow *mainWindow); virtual ~ConnectionManager(); void init(); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index d57393baf..849fa1828 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -177,7 +177,8 @@ MainWindow::MainWindow() : #endif m_modeManager = new ModeManager(this, m_modeStack); - m_connectionManager = new ConnectionManager(this, m_modeStack); + m_connectionManager = new ConnectionManager(this); + m_modeStack->setCornerWidget(m_connectionManager, Qt::TopRightCorner); m_messageManager = new MessageManager; setCentralWidget(m_modeStack); @@ -1214,8 +1215,9 @@ void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly) createWorkspaces(qs); - // Read tab ordering + // Restore tab ordering qs->beginGroup(QLatin1String(modePriorities)); + QStringList modeNames = qs->childKeys(); QMap map; foreach(QString modeName, modeNames) { @@ -1224,6 +1226,12 @@ void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly) m_modeManager->reorderModes(map); qs->endGroup(); + + // Restore selected tab + if (m_workspaceSettings->restoreSelectedOnStartup()) { + int index = qs->value(QLatin1String("SelectedWorkspace")).toInt(); + m_modeStack->setCurrentIndex(index); + } } @@ -1262,12 +1270,16 @@ void MainWindow::saveSettings(QSettings *qs) } qs->endGroup(); + // Write selected tab + qs->setValue(QLatin1String("SelectedWorkspace"), m_modeStack->currentIndex()); + foreach(UAVGadgetManager * manager, m_uavGadgetManagers) { manager->saveSettings(qs); } m_actionManager->saveSettings(qs); m_generalSettings->saveSettings(qs); + qs->beginGroup("General"); qs->setValue("Description", m_config_description); qs->setValue("Details", m_config_details); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp index 1aa28d890..e8431254d 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp @@ -238,6 +238,10 @@ void UAVGadgetInstanceManager::createOptionsPages() while (ite.hasNext()) { IUAVGadgetConfiguration *config = ite.next(); IUAVGadgetFactory *f = factory(config->classId()); + if (!f) { + qWarning() << "No gadget factory for configuration " + config->classId(); + continue; + } IOptionsPage *p = f->createOptionsPage(config); if (p) { IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config, f->isSingleConfigurationGadget()); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp index 43d552f44..d3245f962 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp @@ -33,18 +33,19 @@ #include "ui_workspacesettings.h" - using namespace Core; using namespace Core::Internal; const int WorkspaceSettings::MAX_WORKSPACES = 10; WorkspaceSettings::WorkspaceSettings(QObject *parent) : - IOptionsPage(parent) -{} + IOptionsPage(parent) +{ +} WorkspaceSettings::~WorkspaceSettings() -{} +{ +} // IOptionsPage @@ -84,7 +85,6 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent) m_page->iconPathChooser->setPromptDialogFilter(tr("Images (*.png *.jpg *.bmp *.xpm)")); m_page->iconPathChooser->setPromptDialogTitle(tr("Choose icon")); - connect(m_page->workspaceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectWorkspace(int))); connect(m_page->numberOfWorkspacesSpinBox, SIGNAL(valueChanged(int)), this, SLOT(numberOfWorkspacesChanged(int))); connect(m_page->nameEdit, SIGNAL(textEdited(QString)), this, SLOT(textEdited(QString))); @@ -97,6 +97,7 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent) m_page->comboBoxTabBarPlacement->setCurrentIndex(m_tabBarPlacementIndex); } m_page->checkBoxAllowTabMovement->setChecked(m_allowTabBarMovement); + m_page->checkBoxRestoreSelectedOnStartup->setChecked(m_restoreSelectedOnStartup); return w; } @@ -122,7 +123,10 @@ void WorkspaceSettings::readSettings(QSettings *qs) } m_tabBarPlacementIndex = qs->value(QLatin1String("TabBarPlacementIndex"), 1).toInt(); // 1 == "Bottom" m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool(); + m_restoreSelectedOnStartup = qs->value(QLatin1String("RestoreSelectedOnStartup"), false).toBool(); + qs->endGroup(); + QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } @@ -142,6 +146,7 @@ void WorkspaceSettings::saveSettings(QSettings *qs) } qs->setValue(QLatin1String("TabBarPlacementIndex"), m_tabBarPlacementIndex); qs->setValue(QLatin1String("AllowTabBarMovement"), m_allowTabBarMovement); + qs->setValue(QLatin1String("RestoreSelectedOnStartup"), m_restoreSelectedOnStartup); qs->endGroup(); } @@ -166,6 +171,8 @@ void WorkspaceSettings::apply() } m_tabBarPlacementIndex = m_page->comboBoxTabBarPlacement->currentIndex(); m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked(); + m_restoreSelectedOnStartup = m_page->checkBoxRestoreSelectedOnStartup->isChecked(); + QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South; emit tabBarSettingsApplied(pos, m_allowTabBarMovement); } diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h index 37d2897ae..de2c96732 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h @@ -77,6 +77,10 @@ public: { return m_modeNames.at(i); } + bool restoreSelectedOnStartup() const + { + return m_restoreSelectedOnStartup; + } signals: void tabBarSettingsApplied(QTabWidget::TabPosition pos, bool movable); @@ -98,6 +102,7 @@ private: int m_numberOfWorkspaces; int m_tabBarPlacementIndex; bool m_allowTabBarMovement; + bool m_restoreSelectedOnStartup; static const int MAX_WORKSPACES; }; } // namespace Internal diff --git a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui index 206f7acdd..4be345c22 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui +++ b/ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui @@ -47,62 +47,20 @@ 0 - - - - - 0 - 0 - - - - Details - - - - - - Icon: - - - - - - - - - - - - - Name: - - - - - - - Workspace panel + General - + Placement: - - - - Allow reordering: - - - - + @@ -125,14 +83,7 @@ - - - - - - - - + Qt::Horizontal @@ -145,6 +96,37 @@ + + + + Allow reordering: + + + + + + + + + + + + + + Restore last selected workspace on startup + + + Remember last used workspace on restart + + + + + + + + + + @@ -216,6 +198,41 @@ + + + + + 0 + 0 + + + + Details + + + + + + Icon: + + + + + + + + + + + + + Name: + + + + + + From d99790be71368eb471a8604db758c6ea9ea09a92 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 31 Jul 2013 15:23:43 +0200 Subject: [PATCH 02/11] OP-1058: Implement explicit item accessor for MultiElement fields. This implementation uses a union containing the Array implementing the MultiElement field data and a struct made from element names. it allow to replace the following sintax: settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP] with a more concise and less error prone settings.PitchRatePID.fields.Kp while allowing the direct array access using the notation settings.PitchRatePID.data[n] --- flight/uavobjects/inc/uavobject.h.template | 1 + .../flight/uavobjectgeneratorflight.cpp | 36 ++++++++++++++++--- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/flight/uavobjects/inc/uavobject.h.template b/flight/uavobjects/inc/uavobject.h.template index 971c2d567..572317f01 100644 --- a/flight/uavobjects/inc/uavobject.h.template +++ b/flight/uavobjects/inc/uavobject.h.template @@ -51,6 +51,7 @@ int32_t $(NAME)Initialize(); UAVObjHandle $(NAME)Handle(); void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId); +$(DATASTRUCTURES) /* * Packed Object data (unaligned). * Should only be used where 4 byte alignment can be guaranteed diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 2bc7623e7..9722fc50d 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -118,19 +118,39 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) // Replace the $(DATAFIELDS) tag QString type; QString fields; + QString dataStructures; for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrC[info->fields[n]->type]; // Append field + // Check if it a named set and creates structures accordingly if (info->fields[n]->numElements > 1) { + if(info->fields[n]->elementNames[0].compare(QString("0")) != 0){ + QString unionTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); + QString unionType = QString("typedef union {\n"); + unionType.append(QString(" %1 data[%2];\n").arg(type).arg(info->fields[n]->numElements)); + unionType.append(QString(" struct __attribute__ ((__packed__)) {\n")); + for(int f =0; f < info->fields[n]->elementNames.count(); f++){ + unionType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f])); + } + unionType.append(QString(" } fields;\n")); + unionType.append(QString("} %1 ;\n\n").arg(unionTypeName)); + + dataStructures.append(unionType); + + fields.append(QString(" %1 %2;\n").arg(unionTypeName) + .arg(info->fields[n]->name)); + + } else { fields.append(QString(" %1 %2[%3];\n").arg(type) .arg(info->fields[n]->name).arg(info->fields[n]->numElements)); + } } else { fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); } } outInclude.replace(QString("$(DATAFIELDS)"), fields); - + outInclude.replace(QString("$(DATASTRUCTURES)"), dataStructures); // Replace the $(DATAFIELDINFO) tag QString enums; for (int n = 0; n < info->fields.length(); ++n) { @@ -206,19 +226,27 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) } else { // Initialize all fields in the array for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { + QString optIdentifier; + if(info->fields[n]->elementNames[0].compare(QString("0")) != 0){ + optIdentifier = QString(".data"); + } + if (info->fields[n]->type == FIELDTYPE_ENUM) { - initfields.append(QString(" data.%1[%2] = %3;\n") + initfields.append(QString(" data.%1%2[%3] = %4;\n") .arg(info->fields[n]->name) + .arg(optIdentifier) .arg(idx) .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { - initfields.append(QString(" data.%1[%2] = %3f;\n") + initfields.append(QString(" data.%1%2[%3] = %4f;\n") .arg(info->fields[n]->name) + .arg(optIdentifier) .arg(idx) .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); } else { - initfields.append(QString(" data.%1[%2] = %3;\n") + initfields.append(QString(" data.%1%2[%3] = %4;\n") .arg(info->fields[n]->name) + .arg(optIdentifier) .arg(idx) .arg(info->fields[n]->defaultValues[idx].toInt())); } From 9e1acc3165f4e5b9373e8c7f9aa969a94c4e66dd Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 31 Jul 2013 15:24:26 +0200 Subject: [PATCH 03/11] OP-1058: fix needed for fw_revolution code compilation --- flight/libraries/alarms.c | 16 +- flight/modules/AltitudeHold/altitudehold.c | 6 +- flight/modules/Battery/battery.c | 8 +- flight/modules/CameraStab/camerastab.c | 32 ++-- .../fixedwingpathfollower.c | 150 +++++++++--------- flight/modules/ManualControl/manualcontrol.c | 128 +++++++-------- flight/modules/PathPlanner/pathplanner.c | 34 ++-- flight/modules/Sensors/sensors.c | 46 +++--- flight/modules/Stabilization/relay_tuning.c | 18 +-- flight/modules/Stabilization/stabilization.c | 64 ++++---- flight/modules/Stabilization/virtualflybar.c | 14 +- flight/modules/StateEstimation/filterekf.c | 76 ++++----- flight/modules/System/systemmod.c | 6 +- flight/modules/TxPID/txpid.c | 86 +++++----- .../VtolPathFollower/vtolpathfollower.c | 96 +++++------ shared/uavobjectdefinition/hwsettings.xml | 2 +- 16 files changed, 391 insertions(+), 391 deletions(-) diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 20e6753eb..54511cf01 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -74,8 +74,8 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if (alarms.Alarm[alarm] != severity) { - alarms.Alarm[alarm] = severity; + if (alarms.Alarm.data[alarm] != severity) { + alarms.Alarm.data[alarm] = severity; SystemAlarmsSet(&alarms); } @@ -109,10 +109,10 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if (alarms.Alarm[alarm] != severity) { - alarms.ExtendedAlarmStatus[alarm] = status; - alarms.ExtendedAlarmSubStatus[alarm] = subStatus; - alarms.Alarm[alarm] = severity; + if (alarms.Alarm.data[alarm] != severity) { + alarms.ExtendedAlarmStatus.data[alarm] = status; + alarms.ExtendedAlarmSubStatus.data[alarm] = subStatus; + alarms.Alarm.data[alarm] = severity; SystemAlarmsSet(&alarms); } @@ -137,7 +137,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) // Read alarm SystemAlarmsGet(&alarms); - return alarms.Alarm[alarm]; + return alarms.Alarm.data[alarm]; } /** @@ -229,7 +229,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) // Go through alarms and check if any are of the given severity or higher for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { - if (alarms.Alarm[n] >= severity) { + if (alarms.Alarm.data[n] >= severity) { xSemaphoreGiveRecursive(lock); return 1; } diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index a886de64c..7cf41feeb 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -392,9 +392,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // shutdown motors stabilizationDesired.Throttle = -1; } - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabilizationDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index cf16cf3de..f52e0298c 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -135,13 +135,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // calculate the battery parameters if (voltageADCPin >= 0) { - flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; // in Volts + flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.fields.VoltageZero) * batterySettings.SensorCalibrations.fields.VoltageFactor; // in Volts } else { flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >= 0) { - flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; // in Amps + flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.fields.CurrentZero) * batterySettings.SensorCalibrations.fields.CurrentFactor; // in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps } @@ -184,9 +184,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_ALARM] * batterySettings.NbCells) { + if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Alarm * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); - } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_WARNING] * batterySettings.NbCells) { + } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Warning * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 0088c4015..d616eb935 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -171,17 +171,17 @@ static void attitudeUpdated(UAVObjEvent *ev) // process axes for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { // read and process control input - if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + if (cameraStab.Input.data[i] != CAMERASTABSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(cameraStab.Input.data[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { float input_rate; - switch (cameraStab.StabilizationMode[i]) { + switch (cameraStab.StabilizationMode.data[i]) { case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: - csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; + csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange.data[i]; break; case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: - input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; + input_rate = accessory.AccessoryVal * cameraStab.InputRate.data[i]; if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { - csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); + csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange.data[i]); } break; default: @@ -208,14 +208,14 @@ static void attitudeUpdated(UAVObjEvent *ev) } #ifdef USE_GIMBAL_LPF - if (cameraStab.ResponseTime) { - float rt = (float)cameraStab.ResponseTime[i]; + if (cameraStab.ResponseTime.data) { + float rt = (float)cameraStab.ResponseTime.data[i]; attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); } #endif #ifdef USE_GIMBAL_FF - if (cameraStab.FeedForward[i]) { + if (cameraStab.FeedForward.data[i]) { applyFeedForward(i, dT_millis, &attitude, &cameraStab); } #endif @@ -223,7 +223,7 @@ static void attitudeUpdated(UAVObjEvent *ev) // bounding for elevon mixing occurs on the unmixed output // to limit the range of the mixed output you must limit the range // of both the unmixed pitch and unmixed roll - float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); + float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange.data[i], 1.0f); // set output channels switch (i) { @@ -298,16 +298,16 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta if (index == CAMERASTABSETTINGS_INPUT_ROLL) { float pitch; AttitudeStatePitchGet(&pitch); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + gimbalTypeCorrection = (cameraStab->OutputRange.fields.Pitch - fabsf(pitch)) + / cameraStab->OutputRange.fields.Pitch; } break; case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: if (index == CAMERASTABSETTINGS_INPUT_PITCH) { float roll; AttitudeStateRollGet(&roll); - gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) - / cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + gimbalTypeCorrection = (cameraStab->OutputRange.fields.Roll - fabsf(roll)) + / cameraStab->OutputRange.fields.Roll; } break; default: @@ -316,11 +316,11 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta // apply feed forward float accumulator = csd->ffFilterAccumulator[index]; - accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; + accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward.data[index] * gimbalTypeCorrection; csd->ffLastAttitude[index] = *attitude; *attitude += accumulator; - float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; + float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime.data[index] : cameraStab->DecelTime.data[index]) / dT_millis; if (filter < 1.0f) { filter = 1.0f; } diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 8952164d6..f6af6717e 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -261,7 +261,7 @@ static void updatePathVelocity() positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); float groundspeed; float altitudeSetpoint; @@ -271,7 +271,7 @@ static void updatePathVelocity() case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity; - altitudeSetpoint = pathDesired.End[2]; + altitudeSetpoint = pathDesired.End.fields.Down; break; case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: @@ -280,7 +280,7 @@ static void updatePathVelocity() default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); - altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * + altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * bound(progress.fractional_progress, 0, 1); break; } @@ -358,9 +358,9 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; StabilizationDesiredSet(&stabDesired); } @@ -444,29 +444,29 @@ static uint8_t updateFixedDesiredAttitude() descentspeedError = descentspeedDesired - velocityState.Down; // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_OVERSPEED]) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; + fixedwingpathfollowerStatus.Errors.fields.Highspeed = 0; + fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 0; + if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.fields.Overspeed) { + fixedwingpathfollowerStatus.Errors.fields.Overspeed = 1; result = 0; } - if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHSPEED]) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; + if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.fields.Highspeed) { + fixedwingpathfollowerStatus.Errors.fields.Highspeed = 1; result = 0; } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWSPEED]) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.fields.Lowspeed) { + fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; result = 0; } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_STALLSPEED]) { - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.fields.Stallspeed) { + fixedwingpathfollowerStatus.Errors.fields.Stallspeed = 1; result = 0; } if (indicatedAirspeedState < 1e-6f) { // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // also we cannot handle planes flying backwards, lets just wait until the nose drops - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; + fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; return 0; } @@ -474,53 +474,53 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired throttle command */ // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] > 0) { + if (fixedwingpathfollowerSettings.PowerPI.fields.Ki > 0) { powerIntegral = bound(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], - fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + -fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki, + fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } else { powerIntegral = 0; } // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = bound( - (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] + (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Kp, + -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max, + fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max ); // Compute final throttle response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + - powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.fields.Kp + + powerIntegral * fixedwingpathfollowerSettings.PowerPI.fields.Ki + speedErrorToPowerCommandComponent; // Output internal state to telemetry - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; + fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError; + fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral; + fixedwingpathfollowerStatus.Command.fields.Power = powerCommand; // set throttle - stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); + stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand, + fixedwingpathfollowerSettings.ThrottleLimit.fields.Min, + fixedwingpathfollowerSettings.ThrottleLimit.fields.Max); // Error condition: plane cannot hold altitude at current speed. - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum + fixedwingpathfollowerStatus.Errors.fields.Lowpower = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Max && // throttle at maximum velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError > 0 && // we are too slow already - fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWPOWER] > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; + fixedwingpathfollowerSettings.Safetymargins.fields.Lowpower > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.fields.Lowpower = 1; result = 0; } // Error condition: plane keeps climbing despite minimum throttle (opposite of above) - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum + fixedwingpathfollowerStatus.Errors.fields.Highpower = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Min && // throttle at minimum velocityState.Down < 0 && // we ARE going up descentspeedDesired > 0 && // we WANT to go down airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; + fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.fields.Highpower = 1; result = 0; } @@ -529,40 +529,40 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired pitch command */ - if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) { + if (fixedwingpathfollowerSettings.SpeedPI.fields.Ki > 0) { // Integrate with saturation airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], - fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); + -fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki, + fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki); } // Compute the cross feed from vertical speed to pitch, with saturation - float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] + float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Kp, + -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max, + fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max ); // Compute the pitch command as err*Kp + errInt*Ki + X_feed. - pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] - + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] + pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.fields.Kp + + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki ) + verticalSpeedToPitchCommandComponent; - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; + fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError; + fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt; + fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand; - stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], - fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit.fields.Min, + fixedwingpathfollowerSettings.PitchLimit.fields.Max); // Error condition: high speed dive - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; - if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up + fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 0; + if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.fields.Max && // pitch demand is full up velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_PITCHCONTROL] > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; + fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1; result = 0; } @@ -579,12 +579,12 @@ static uint8_t updateFixedDesiredAttitude() headingError -= 360.0f; } // Error condition: wind speed is higher than airspeed. We are forced backwards! - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if ((headingError > fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND] || - headingError < -fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND]) && - fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.fields.Wind = 0; + if ((headingError > fixedwingpathfollowerSettings.Safetymargins.fields.Wind || + headingError < -fixedwingpathfollowerSettings.Safetymargins.fields.Wind) && + fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on // we are flying backwards - fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; + fixedwingpathfollowerStatus.Errors.fields.Wind = 1; result = 0; } @@ -608,20 +608,20 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } - courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI], - -fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT], - fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]); - courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] + + courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.fields.Ki, + -fixedwingpathfollowerSettings.CoursePI.fields.ILimit, + fixedwingpathfollowerSettings.CoursePI.fields.ILimit); + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp + courseIntegral); - fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_COURSE] = courseError; - fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_COURSE] = courseIntegral; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_COURSE] = courseCommand; + fixedwingpathfollowerStatus.Error.fields.Course = courseError; + fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral; + fixedwingpathfollowerStatus.Command.fields.Course = courseCommand; - stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral + courseCommand, - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], - fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]); + fixedwingpathfollowerSettings.RollLimit.fields.Min, + fixedwingpathfollowerSettings.RollLimit.fields.Max); // TODO: find a check to determine loss of directional control. Likely needs some check of derivative @@ -633,9 +633,9 @@ static uint8_t updateFixedDesiredAttitude() stabDesired.Yaw = 0; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; StabilizationDesiredSet(&stabDesired); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index dcb61ff82..aa1bd1e1d 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -245,10 +245,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { extern uint32_t pios_rcvr_group_map[]; - if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { cmd.Channel[n] = PIOS_RCVR_INVALID; } else { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]); + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups.data[n]], settings.ChannelNumber.data[n]); } // If a channel has timed out this is not valid data and we shouldn't update anything @@ -256,15 +256,15 @@ static void manualControlTask(__attribute__((unused)) void *parameters) if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { valid_input_detected = false; } else { - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax.data[n], settings.ChannelMin.data[n], settings.ChannelNeutral.data[n]); } } // Check settings, if error raise alarm - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID @@ -281,7 +281,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + && (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -296,14 +296,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + valid_input_detected &= validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], + settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], + settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], + settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], + settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -333,21 +333,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -389,7 +389,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); @@ -406,7 +406,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 1 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); @@ -423,7 +423,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 2 - if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); @@ -671,13 +671,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; + stab_settings = settings->Stabilization1Settings.data; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; + stab_settings = settings->Stabilization2Settings.data; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; + stab_settings = settings->Stabilization3Settings.data; break; default: // Major error, this should not occur because only enter this block when one of these is true @@ -686,44 +686,44 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + stabilization.StabilizationMode.fields.Roll = stab_settings[0]; + stabilization.StabilizationMode.fields.Pitch = stab_settings[1]; + stabilization.StabilizationMode.fields.Yaw = stab_settings[2]; stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll: (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll: (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.fields.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + cmd->Pitch * stabSettings.ManualRate.fields.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + cmd->Pitch * stabSettings.ManualRate.fields.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + cmd->Pitch * stabSettings.ManualRate.fields.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; @@ -754,12 +754,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_START_NORTH] = 0; - pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End[PATHDESIRED_END_NORTH] = 0; - pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.Start.fields.North = 0; + pathDesired.Start.fields.East = 0; + pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.fields.North = 0; + pathDesired.End.fields.East = 0; + pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -771,12 +771,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; + pathDesired.Start.fields.North = positionState.North; + pathDesired.Start.fields.East = positionState.East; + pathDesired.Start.fields.Down = positionState.Down; + pathDesired.End.fields.North = positionState.North; + pathDesired.End.fields.East = positionState.East; + pathDesired.End.fields.Down = positionState.Down; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -813,17 +813,17 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredGet(&pathDesired); if (changed) { // After not being in this mode for a while init at current height - pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; - pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; + pathDesired.Start.fields.North = positionState.North; + pathDesired.Start.fields.East = positionState.East; + pathDesired.Start.fields.Down = positionState.Down; + pathDesired.End.fields.North = positionState.North; + pathDesired.End.fields.East = positionState.East; + pathDesired.End.fields.Down = positionState.Down; pathDesired.StartingVelocity = 1; pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down + 5; + pathDesired.End.fields.Down = positionState.Down + 5; PathDesiredSet(&pathDesired); } @@ -875,7 +875,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.fields.Yaw; float currentDown; PositionStateDownGet(¤tDown); @@ -974,7 +974,7 @@ static bool okToArm(void) // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (alarms.Alarm.data[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; } @@ -1007,7 +1007,7 @@ static bool forcedDisArm(void) SystemAlarmsGet(&alarms); - if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { + if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { return true; } return false; @@ -1246,8 +1246,8 @@ static void applyDeadband(float *value, float deadband) */ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) { - if (settings->ResponseTime[channel]) { - float rt = (float)settings->ResponseTime[channel]; + if (settings->ResponseTime.data[channel]) { + float rt = (float)settings->ResponseTime.data[channel]; inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); *value = inputFiltered[channel]; } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index c7be26409..003d97a3b 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -227,9 +227,9 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) WaypointInstGet(waypointActiveData.Index, &waypointData); PathActionInstGet(waypointData.Action, &pathActionData); - pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; - pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.End.fields.North = waypointData.Position.fields.North; + pathDesired.End.fields.East = waypointData.Position.fields.East; + pathDesired.End.fields.Down = waypointData.Position.fields.Down; pathDesired.EndingVelocity = waypointData.Velocity; pathDesired.Mode = pathActionData.Mode; pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; @@ -246,18 +246,18 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ - pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; + pathDesired.Start.fields.North = positionState.North; + pathDesired.Start.fields.East = positionState.East; + pathDesired.Start.fields.Down = positionState.Down; pathDesired.StartingVelocity = pathDesired.EndingVelocity; } else { // Get previous waypoint as start point WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.Start.fields.North = waypointPrev.Position.fields.North; + pathDesired.Start.fields.East = waypointPrev.Position.fields.East; + pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down; pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); @@ -369,12 +369,12 @@ static uint8_t conditionDistanceToTarget() PositionStateGet(&positionState); if (pathAction.ConditionParameters[1] > 0.5f) { - distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) - + powf(waypoint.Position[1] - positionState.East, 2) - + powf(waypoint.Position[2] - positionState.Down, 2)); + distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2) + + powf(waypoint.Position.fields.East - positionState.East, 2) + + powf(waypoint.Position.fields.Down - positionState.Down, 2)); } else { - distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) - + powf(waypoint.Position[1] - positionState.East, 2)); + distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2) + + powf(waypoint.Position.fields.East - positionState.East, 2)); } if (distance <= pathAction.ConditionParameters[0]) { @@ -400,7 +400,7 @@ static uint8_t conditionLegRemaining() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -423,7 +423,7 @@ static uint8_t conditionBelowError() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } @@ -492,7 +492,7 @@ static uint8_t conditionPointingTowardsNext() WaypointData nextWaypoint; WaypointInstGet(nextWaypointId, &nextWaypoint); - float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1])); + float angle1 = atan2f((nextWaypoint.Position.fields.North - waypoint.Position.fields.North), (nextWaypoint.Position.fields.East - waypoint.Position.fields.East)); VelocityStateData velocity; VelocityStateGet(&velocity); diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 3b2397b90..0f716848c 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -422,38 +422,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { RevoCalibrationGet(&cal); - mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; - mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; - mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; - mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; - mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; - mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; - accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; - accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; - accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; - accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; - accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; - accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; - gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; - gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; - gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; - gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; - gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; - gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; + mag_bias[0] = cal.mag_bias.fields.X; + mag_bias[1] = cal.mag_bias.fields.Y; + mag_bias[2] = cal.mag_bias.fields.Z; + mag_scale[0] = cal.mag_scale.fields.X; + mag_scale[1] = cal.mag_scale.fields.Y; + mag_scale[2] = cal.mag_scale.fields.Z; + accel_bias[0] = cal.accel_bias.fields.X; + accel_bias[1] = cal.accel_bias.fields.Y; + accel_bias[2] = cal.accel_bias.fields.Z; + accel_scale[0] = cal.accel_scale.fields.X; + accel_scale[1] = cal.accel_scale.fields.Y; + accel_scale[2] = cal.accel_scale.fields.Z; + gyro_staticbias[0] = cal.gyro_bias.fields.X; + gyro_staticbias[1] = cal.gyro_bias.fields.Y; + gyro_staticbias[2] = cal.gyro_bias.fields.Z; + gyro_scale[0] = cal.gyro_scale.fields.X; + gyro_scale[1] = cal.gyro_scale.fields.Y; + gyro_scale[2] = cal.gyro_scale.fields.Z; AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); // Indicates not to expend cycles on rotation - if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { + if (attitudeSettings.BoardRotation.fields.Roll== 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 && + attitudeSettings.BoardRotation.fields.Yaw == 0) { rotate = 0; } else { float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, + attitudeSettings.BoardRotation.fields.Pitch , + attitudeSettings.BoardRotation.fields.Yaw}; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index 7fc25aa3e..6b01b8f57 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -71,8 +71,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) // On first run initialize estimates to something reasonable if (reinit) { rateRelayRunning[axis] = false; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; + relay.Period.data[axis] = 200; + relay.Gain.data[axis] = 0; accum_sin = 0; accum_cos = 0; @@ -95,14 +95,14 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) /**** The code below here is to estimate the properties of the oscillation ****/ // Make sure the period can't go below limit - if (relay.Period[axis] < DEGLITCH_TIME) { - relay.Period[axis] = DEGLITCH_TIME; + if (relay.Period.data[axis] < DEGLITCH_TIME) { + relay.Period.data[axis] = DEGLITCH_TIME; } // Project the error onto a sine and cosine of the same frequency // to accumulate the average amplitude int32_t dT = thisTime - lastHighTime; - float phase = ((float)360 * (float)dT) / relay.Period[axis]; + float phase = ((float)360 * (float)dT) / relay.Period.data[axis]; if (phase >= 360) { phase = 0; } @@ -125,12 +125,12 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) if (rateRelayRunning[axis] == false) { rateRelayRunning[axis] = true; - relay.Period[axis] = 200; - relay.Gain[axis] = 0; + relay.Period.data[axis] = 200; + relay.Gain.data[axis] = 0; } else { // Low pass filter each amplitude and period - relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); - relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + relay.Period.data[axis] = relay.Period.data[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); } lastHighTime = thisTime; high = true; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 6cfeed50f..3e5cbca3b 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -248,18 +248,18 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Run the selected stabilization algorithm on each axis: for (uint8_t i = 0; i < MAX_AXES; i++) { // Check whether this axis mode needs to be reinitialized - bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); - previous_mode[i] = stabDesired.StabilizationMode[i]; + bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]); + previous_mode[i] = stabDesired.StabilizationMode.data[i]; // Apply the selected control law - switch (stabDesired.StabilizationMode[i]) { + switch (stabDesired.StabilizationMode.data[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: if (reinit) { pids[PID_RATE_ROLL + i].iAccumulator = 0; } // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); @@ -275,7 +275,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); @@ -326,7 +326,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate.data[i]); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); @@ -335,7 +335,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); @@ -350,7 +350,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop like attitude mode rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); @@ -456,37 +456,37 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) StabilizationSettingsGet(&settings); // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], - settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], - pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], - pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp, + settings.RollRatePID.fields.Ki, + pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd, + pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], - pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], - pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); + pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp, + pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki, + pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd, + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], - pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], - pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], - pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); + pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp, + pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki, + pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd, + pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit); // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], - settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, - pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); + pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp, + settings.RollPI.fields.Ki, 0, + pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit); // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], - pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, - settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); + pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp, + pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0, + settings.PitchPI.fields.ILimit); // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp, + settings.YawPI.fields.Ki, 0, + settings.YawPI.fields.ILimit); // Set up the derivative term pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); @@ -503,9 +503,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll== STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 153615c57..1e7c7baab 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -64,23 +64,23 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float // Get the settings for the correct axis switch (axis) { case ROLL: - kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + kp = settings->VbarRollPI.fields.Kp; + ki = settings->VbarRollPI.fields.Ki; break; case PITCH: - kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + kp = settings->VbarPitchPI.fields.Kp; + ki = settings->VbarPitchPI.fields.Ki;; break; case YAW: - kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; - ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; + kp = settings->VbarYawPI.fields.Kp; + ki = settings->VbarYawPI.fields.Ki; break; default: PIOS_DEBUG_Assert(0); } // Command signal is composed of stick input added to the gyro and virtual flybar - *output = command * settings->VbarSensitivity[axis] - + *output = command * settings->VbarSensitivity.data[axis] - gyro_gain * (vbar_integral[axis] * ki + gyro * kp); return 0; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 239dd0911..2de51506d 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -163,17 +163,17 @@ static int32_t maininit(stateFilter *self) int t; // plausibility check for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.P[t])) { + if (invalid_var(this->ekfConfiguration.P.data[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.Q[t])) { + if (invalid_var(this->ekfConfiguration.Q.data[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.R[t])) { + if (invalid_var(this->ekfConfiguration.R.data[t])) { return 2; } } @@ -242,23 +242,23 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSGPSInit(); // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2 float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2]; - INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / Be2, - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2, - this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 } + INSSetMagVar((float[3]) { this->ekfConfiguration.R.fields.MagX / Be2, + this->ekfConfiguration.R.fields.MagY / Be2, + this->ekfConfiguration.R.fields.MagZ / Be2 } ); - INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.fields.AccelX, + this->ekfConfiguration.Q.fields.AccelY, + this->ekfConfiguration.Q.fields.AccelZ } ); - INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.fields.GyroX, + this->ekfConfiguration.Q.fields.GyroY, + this->ekfConfiguration.Q.fields.GyroZ } ); - INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], - this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.fields.GyroDriftX, + this->ekfConfiguration.Q.fields.GyroDriftY, + this->ekfConfiguration.Q.fields.GyroDriftZ } ); - INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + INSSetBaroVar(this->ekfConfiguration.R.fields.BaroZ); // Initialize the gyro bias float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; @@ -294,7 +294,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros); - INSResetP(this->ekfConfiguration.P); + INSResetP(this->ekfConfiguration.P.data); } else { // Run prediction a bit before any corrections @@ -368,21 +368,21 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (!this->usePos) { // position and velocity variance used in indoor mode - INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, + (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor } ); } else { // position and velocity variance used in outdoor mode - INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], - this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], - this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, - (float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], - this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], - this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.fields.GPSPosNorth, + this->ekfConfiguration.R.fields.GPSPosEast, + this->ekfConfiguration.R.fields.GPSPosDown }, + (float[3]) { this->ekfConfiguration.R.fields.GPSVelNorth, + this->ekfConfiguration.R.fields.GPSVelEast, + this->ekfConfiguration.R.fields.GPSVelDown } ); } @@ -397,12 +397,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, + (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, + this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, + this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed } ); // rotate airspeed vector into NED frame - airspeed is measured in X axis only float R[3][3]; @@ -421,12 +421,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(vardata.P); + INSGetP(vardata.P.data); EKFStateVarianceSet(&vardata); int t; for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { - if (!IS_REAL(vardata.P[t]) || vardata.P[t] <= 0.0f) { - INSResetP(this->ekfConfiguration.P); + if (!IS_REAL(vardata.P.data[t]) || vardata.P.data[t] <= 0.0f) { + INSResetP(this->ekfConfiguration.P.data); this->init_stage = -1; break; } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 792874756..316f5db8c 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -426,9 +426,9 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_ // By convention, there is a direct mapping between task monitor task_id's and members // of the TaskInfoXXXXElem enums PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); - taskData->Running[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; - taskData->StackRemaining[task_id] = task_info->stack_remaining; - taskData->RunningTime[task_id] = task_info->running_time_percentage; + taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; + taskData->StackRemaining.data[task_id] = task_info->stack_remaining; + taskData->RunningTime.data[task_id] = task_info->running_time_percentage; } #endif diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 96ffdd156..6b180fd3a 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -171,111 +171,111 @@ static void updatePIDs(UAVObjEvent *ev) // Loop through every enabled instance for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { - if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { + if (inst.PIDs.data[i] != TXPIDSETTINGS_PIDS_DISABLED) { float value; - if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { + if (inst.Inputs.data[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { ManualControlCommandThrottleGet(&value); value = scale(value, - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], - inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], - inst.MinPID[i], inst.MaxPID[i]); - } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { - value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); + inst.ThrottleRange.fields.Min, + inst.ThrottleRange.fields.Max, + inst.MinPID.data[i], inst.MaxPID.data[i]); + } else if (AccessoryDesiredInstGet(inst.Inputs.data[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { + value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID.data[i], inst.MaxPID.data[i]); } else { continue; } - switch (inst.PIDs[i]) { + switch (inst.PIDs.data[i]) { case TXPIDSETTINGS_PIDS_ROLLRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); + needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); + needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); + needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); + needsUpdate |= update(&stab.RollPI.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); + needsUpdate |= update(&stab.RollPI.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); + needsUpdate |= update(&stab.RollPI.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKP: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKI: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKD: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + needsUpdate |= update(&stab.PitchPI.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + needsUpdate |= update(&stab.PitchPI.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); + needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); + needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); + needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); + needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); + needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); + needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); - needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); + needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); + needsUpdate |= update(&stab.RollPI.fields.Kp, value); + needsUpdate |= update(&stab.PitchPI.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); + needsUpdate |= update(&stab.RollPI.fields.Ki, value); + needsUpdate |= update(&stab.PitchPI.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); - needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); + needsUpdate |= update(&stab.RollPI.fields.ILimit, value); + needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKP: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); + needsUpdate |= update(&stab.YawRatePID.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKI: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); + needsUpdate |= update(&stab.YawRatePID.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKD: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); + needsUpdate |= update(&stab.YawRatePID.fields.Kd, value); break; case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: - needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); + needsUpdate |= update(&stab.YawRatePID.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); + needsUpdate |= update(&stab.YawPI.fields.Kp, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); + needsUpdate |= update(&stab.YawPI.fields.Ki, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: - needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); + needsUpdate |= update(&stab.YawPI.fields.ILimit, value); break; case TXPIDSETTINGS_PIDS_GYROTAU: needsUpdate |= update(&stab.GyroTau, value); diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 63829b434..f2f8421f8 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -337,8 +337,8 @@ static void updatePOIBearing() // don't try to move any closer if (poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.fields.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.fields.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -351,7 +351,7 @@ static void updatePOIBearing() /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ stabDesired.Yaw = yaw + (pathAngle / 2.0f); - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // cameraDesired.Yaw=yaw; // cameraDesired.PitchOrServo2=elevation; @@ -382,7 +382,7 @@ static void updatePathVelocity() { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); + path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); float groundspeed; switch (pathDesired.Mode) { @@ -414,7 +414,7 @@ static void updatePathVelocity() velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; - float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; + float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp; float correction_velocity[2] = { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; @@ -427,13 +427,13 @@ static void updatePathVelocity() velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); + float altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * bound(progress.fractional_progress, 0, 1); float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki, + -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit, + vtolpathfollowerSettings.VerticalPosPI.fields.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus @@ -505,17 +505,17 @@ void updateEndpointVelocity() } // Compute desired north command - northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; - northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); + northError = pathDesired.End.fields.North - northPos; + northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit); + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + northPosIntegral); - eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], - -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], - vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); + eastError = pathDesired.End.fields.East - eastPos; + eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + eastPosIntegral); // Limit the maximum velocity float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); @@ -527,11 +527,11 @@ void updateEndpointVelocity() velocityDesired.North = northCommand * scale; velocityDesired.East = eastCommand * scale; - downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], - -vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], - vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); + downError = pathDesired.End.fields.Down - downPos; + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki, + -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit, + vtolpathfollowerSettings.VerticalPosPI.fields.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); @@ -550,9 +550,9 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; StabilizationDesiredSet(&stabDesired); } @@ -629,31 +629,31 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Compute desired north command northError = velocityDesired.North - northVel; - northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit); + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + northVelIntegral + - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired east command eastError = velocityDesired.East - eastVel; - eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], - -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], - vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] + eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + eastVelIntegral + - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired down command downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; - downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], - -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], - vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); + downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.fields.Ki, + -vtolpathfollowerSettings.VerticalVelPID.fields.ILimit, + vtolpathfollowerSettings.VerticalVelPID.fields.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.fields.Kp + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.fields.Kd); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); @@ -673,13 +673,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.Throttle = manualControl.Throttle; } - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; + stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.fields.Yaw * manualControlData.Yaw; } StabilizationDesiredSet(&stabDesired); } diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 422695c21..a1bc068e8 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -23,7 +23,7 @@ - + From df90a13558d14e2b4bd67ba9da397bbdec29c81b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 1 Aug 2013 01:18:26 +0200 Subject: [PATCH 04/11] OP-1058: fix needed for fw_coptercontrol code compilation --- flight/modules/Attitude/attitude.c | 28 ++++++++++---------- flight/modules/ManualControl/manualcontrol.c | 4 +-- flight/modules/Stabilization/stabilization.c | 6 ++--- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 81c1f947d..d3c88e009 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -609,17 +609,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; - accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X]; - accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y]; - accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z]; + accelbias[0] = attitudeSettings.AccelBias.fields.X; + accelbias[1] = attitudeSettings.AccelBias.fields.Y; + accelbias[2] = attitudeSettings.AccelBias.fields.Z; - gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f; - gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f; - gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f; + gyro_correct_int[0] = attitudeSettings.GyroBias.fields.X / 100.0f; + gyro_correct_int[1] = attitudeSettings.GyroBias.fields.Y / 100.0f; + gyro_correct_int[2] = attitudeSettings.GyroBias.fields.Z / 100.0f; // Indicates not to expend cycles on rotation - if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && - attitudeSettings.BoardRotation[2] == 0) { + if (attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Roll == 0 && + attitudeSettings.BoardRotation.fields.Yaw == 0) { rotate = 0; // Shouldn't be used but to be safe @@ -627,9 +627,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) Quaternion2R(rotationQuat, R); } else { float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], - attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; + const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, + attitudeSettings.BoardRotation.fields.Pitch, + attitudeSettings.BoardRotation.fields.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; @@ -643,10 +643,10 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) trim_requested = true; } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { trim_requested = false; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples; - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples; + attitudeSettings.AccelBias.fields.X = trim_accels[0] / trim_samples; + attitudeSettings.AccelBias.fields.Y = trim_accels[1] / trim_samples; // Z should average -grav - attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; + attitudeSettings.AccelBias.fields.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; AttitudeSettingsSet(&attitudeSettings); } else { diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index aa1bd1e1d..7e7ad5062 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -453,8 +453,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters) if (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, cmd.Channel, - settings.ChannelMin, - settings.ChannelMax, + settings.ChannelMin.data, + settings.ChannelMax.data, NELEMENTS(cmd.Channel)); } #endif /* PIOS_INCLUDE_USB_RCTX */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 3e5cbca3b..a95b39dd4 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -192,19 +192,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float local_error[3]; // Essentially zero errors for anything in rate or none - if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.fields.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[0] = stabDesired.Roll; } else { rpy_desired[0] = attitudeState.Roll; } - if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.fields.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[1] = stabDesired.Pitch; } else { rpy_desired[1] = attitudeState.Pitch; } - if (stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.fields.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[2] = stabDesired.Yaw; } else { rpy_desired[2] = attitudeState.Yaw; From 8351e97faa7ee47efa47cb056707edfe4d226337 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 1 Aug 2013 01:18:53 +0200 Subject: [PATCH 05/11] OP-1058: fix needed for fw_revoproto code compilation --- flight/modules/Attitude/revolution/attitude.c | 84 +++++++++---------- .../boards/revoproto/firmware/pios_board.c | 2 +- 2 files changed, 43 insertions(+), 43 deletions(-) diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index fba41c75f..9a9a0449b 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -834,12 +834,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) value_error = true; } - if (invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) || - invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN])) { + if (invalid_var(ekfConfiguration.R.fields.GPSPosNorth) || + invalid_var(ekfConfiguration.R.fields.GPSPosEast) || + invalid_var(ekfConfiguration.R.fields.GPSPosDown) || + invalid_var(ekfConfiguration.R.fields.GPSVelNorth) || + invalid_var(ekfConfiguration.R.fields.GPSVelEast) || + invalid_var(ekfConfiguration.R.fields.GPSVelDown)) { gps_updated = false; value_error = true; } @@ -892,23 +892,23 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if (init_stage == 0) { // Reset the INS algorithm INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGY], - ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] } + INSSetMagVar((float[3]) { ekfConfiguration.R.fields.MagX, + ekfConfiguration.R.fields.MagY, + ekfConfiguration.R.fields.MagZ } ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } + INSSetAccelVar((float[3]) { ekfConfiguration.Q.fields.AccelX, + ekfConfiguration.Q.fields.AccelY, + ekfConfiguration.Q.fields.AccelZ } ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } + INSSetGyroVar((float[3]) { ekfConfiguration.Q.fields.GyroX, + ekfConfiguration.Q.fields.GyroY, + ekfConfiguration.Q.fields.GyroZ } ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], - ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } + INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.fields.GyroDriftX, + ekfConfiguration.Q.fields.GyroDriftY, + ekfConfiguration.Q.fields.GyroDriftZ } ); - INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); + INSSetBaroVar(ekfConfiguration.R.fields.BaroZ); // Initialize the gyro bias float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; @@ -966,7 +966,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; INSSetState(pos, zeros, q, zeros, zeros); - INSResetP(ekfConfiguration.P); + INSResetP(ekfConfiguration.P.data); } else { // Run prediction a bit before any corrections @@ -1028,12 +1028,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, - (float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], - ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } + INSSetPosVelVar((float[3]) { ekfConfiguration.R.fields.GPSPosNorth, + ekfConfiguration.R.fields.GPSPosEast, + ekfConfiguration.R.fields.GPSPosDown }, + (float[3]) { ekfConfiguration.R.fields.GPSVelNorth, + ekfConfiguration.R.fields.GPSVelEast, + ekfConfiguration.R.fields.GPSVelDown } ); sensors |= POS_SENSORS; @@ -1051,12 +1051,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) (1.0f - BARO_OFFSET_LOWPASS_ALPHA) * (-NED[2] - baroData.Altitude); } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, + (float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, + ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, + ekfConfiguration.FakeR.fields.FakeGPSVelIndoor } ); vel[0] = vel[1] = vel[2] = 0.0f; NED[0] = NED[1] = 0.0f; @@ -1084,12 +1084,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) 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[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, - (float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], - ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } + INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, + ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, + (float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, + ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, + ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed } ); // rotate airspeed vector into NED frame - airspeed is measured in X axis only float R[3][3]; @@ -1130,7 +1130,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(vardata.P); + INSGetP(vardata.P.data); EKFStateVarianceSet(&vardata); return 0; @@ -1180,17 +1180,17 @@ static void settingsUpdatedCb(UAVObjEvent *ev) EKFConfigurationGet(&ekfConfiguration); int t; for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.P[t])) { + if (invalid_var(ekfConfiguration.P.data[t])) { error = true; } } for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.Q[t])) { + if (invalid_var(ekfConfiguration.Q.data[t])) { error = true; } } for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.R[t])) { + if (invalid_var(ekfConfiguration.R.data[t])) { error = true; } } diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 11a4f0af5..672eac86e 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -866,7 +866,7 @@ void PIOS_Board_Init(void) { HwSettingsData hwSettings; HwSettingsGet(&hwSettings); - if (hwSettings.OptionalModules[HWSETTINGS_OPTIONALMODULES_OVERO] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (hwSettings.OptionalModules.fields.Overo == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } From f226b23c24deef26b4fe59c742ed9968ac9f5a86 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 1 Aug 2013 01:19:02 +0200 Subject: [PATCH 06/11] OP-1058: fix needed for fw_osd code compilation --- flight/modules/Osd/osdgen/osdgen.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index a197366b1..fa313a9fd 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -2219,8 +2219,8 @@ void updateGraphics() /* Draw Attitude Indicator */ if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { - drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96); + drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.fields.X), + APPLY_VDEADBAND(OsdSettings.AttitudeSetup.fields.Y), attitude.Pitch, attitude.Roll, 96); } // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); // printText16( 60, 12,"Hello OP-OSD"); @@ -2239,7 +2239,7 @@ void updateGraphics() /* Print RTC time */ if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { - printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y])); + printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.fields.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.fields.Y)); } /* Print Number of detected video Lines */ @@ -2292,22 +2292,22 @@ void updateGraphics() // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); // Draw airspeed (left side.) if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { - hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]), - APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); + hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.fields.X), + APPLY_VDEADBAND(OsdSettings.SpeedSetup.fields.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); } // Draw altimeter (right side.) if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { - hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]), - APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0); + hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.fields.X), + APPLY_VDEADBAND(OsdSettings.AltitudeSetup.fields.Y), 100, 20, 100, 7, 12, 15, 500, 0); } // Draw compass. if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { if (attitude.Yaw < 0) { - hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); + hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), + APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); } else { - hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]), - APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0); + hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), + APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); } } } From 01d963affd0fa966f0ebed2a6ec1a9f12af5c57b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 1 Aug 2013 01:28:50 +0200 Subject: [PATCH 07/11] OP-1058 uncrustify --- .../fixedwingpathfollower.c | 18 ++++---- flight/modules/ManualControl/manualcontrol.c | 42 +++++++++---------- flight/modules/PathPlanner/pathplanner.c | 18 ++++---- flight/modules/Sensors/sensors.c | 6 +-- flight/modules/Stabilization/relay_tuning.c | 12 +++--- flight/modules/Stabilization/stabilization.c | 2 +- .../VtolPathFollower/vtolpathfollower.c | 2 +- .../flight/uavobjectgeneratorflight.cpp | 13 +++--- 8 files changed, 56 insertions(+), 57 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index f6af6717e..00681358c 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -358,7 +358,7 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; StabilizationDesiredSet(&stabDesired); @@ -494,9 +494,9 @@ static uint8_t updateFixedDesiredAttitude() speedErrorToPowerCommandComponent; // Output internal state to telemetry - fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError; + fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError; fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral; - fixedwingpathfollowerStatus.Command.fields.Power = powerCommand; + fixedwingpathfollowerStatus.Command.fields.Power = powerCommand; // set throttle stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand, @@ -519,7 +519,7 @@ static uint8_t updateFixedDesiredAttitude() velocityState.Down < 0 && // we ARE going up descentspeedDesired > 0 && // we WANT to go down airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on + fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on fixedwingpathfollowerStatus.Errors.fields.Highpower = 1; result = 0; } @@ -547,9 +547,9 @@ static uint8_t updateFixedDesiredAttitude() + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki ) + verticalSpeedToPitchCommandComponent; - fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError; + fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError; fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt; - fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand; + fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand; stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand, fixedwingpathfollowerSettings.PitchLimit.fields.Min, @@ -561,7 +561,7 @@ static uint8_t updateFixedDesiredAttitude() velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on + fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1; result = 0; } @@ -614,9 +614,9 @@ static uint8_t updateFixedDesiredAttitude() courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp + courseIntegral); - fixedwingpathfollowerStatus.Error.fields.Course = courseError; + fixedwingpathfollowerStatus.Error.fields.Course = courseError; fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral; - fixedwingpathfollowerStatus.Command.fields.Course = courseCommand; + fixedwingpathfollowerStatus.Command.fields.Course = courseCommand; stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral + courseCommand, diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 7e7ad5062..42640f542 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -686,17 +686,17 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode.fields.Roll = stab_settings[0]; + stabilization.StabilizationMode.fields.Roll = stab_settings[0]; stabilization.StabilizationMode.fields.Pitch = stab_settings[1]; - stabilization.StabilizationMode.fields.Yaw = stab_settings[2]; + stabilization.StabilizationMode.fields.Yaw = stab_settings[2]; stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll: + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll: + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : @@ -757,11 +757,11 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * pathDesired.Start.fields.North = 0; pathDesired.Start.fields.East = 0; pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.fields.North = 0; - pathDesired.End.fields.East = 0; - pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.End.fields.North = 0; + pathDesired.End.fields.East = 0; + pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } else if (changed) { @@ -772,13 +772,13 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start.fields.North = positionState.North; - pathDesired.Start.fields.East = positionState.East; - pathDesired.Start.fields.Down = positionState.Down; - pathDesired.End.fields.North = positionState.North; - pathDesired.End.fields.East = positionState.East; - pathDesired.End.fields.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.fields.East = positionState.East; + pathDesired.Start.fields.Down = positionState.Down; + pathDesired.End.fields.North = positionState.North; + pathDesired.End.fields.East = positionState.East; + pathDesired.End.fields.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. @@ -816,11 +816,11 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * pathDesired.Start.fields.North = positionState.North; pathDesired.Start.fields.East = positionState.East; pathDesired.Start.fields.Down = positionState.Down; - pathDesired.End.fields.North = positionState.North; - pathDesired.End.fields.East = positionState.East; - pathDesired.End.fields.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.End.fields.North = positionState.North; + pathDesired.End.fields.East = positionState.East; + pathDesired.End.fields.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } pathDesired.End.fields.Down = positionState.Down + 5; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 003d97a3b..da5ef0225 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -227,9 +227,9 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) WaypointInstGet(waypointActiveData.Index, &waypointData); PathActionInstGet(waypointData.Action, &pathActionData); - pathDesired.End.fields.North = waypointData.Position.fields.North; - pathDesired.End.fields.East = waypointData.Position.fields.East; - pathDesired.End.fields.Down = waypointData.Position.fields.Down; + pathDesired.End.fields.North = waypointData.Position.fields.North; + pathDesired.End.fields.East = waypointData.Position.fields.East; + pathDesired.End.fields.Down = waypointData.Position.fields.Down; pathDesired.EndingVelocity = waypointData.Velocity; pathDesired.Mode = pathActionData.Mode; pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; @@ -247,18 +247,18 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ pathDesired.Start.fields.North = positionState.North; - pathDesired.Start.fields.East = positionState.East; - pathDesired.Start.fields.Down = positionState.Down; - pathDesired.StartingVelocity = pathDesired.EndingVelocity; + pathDesired.Start.fields.East = positionState.East; + pathDesired.Start.fields.Down = positionState.Down; + pathDesired.StartingVelocity = pathDesired.EndingVelocity; } else { // Get previous waypoint as start point WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); pathDesired.Start.fields.North = waypointPrev.Position.fields.North; - pathDesired.Start.fields.East = waypointPrev.Position.fields.East; - pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down; - pathDesired.StartingVelocity = waypointPrev.Velocity; + pathDesired.Start.fields.East = waypointPrev.Position.fields.East; + pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); } diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 0f716848c..77660a63c 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -446,14 +446,14 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) AttitudeSettingsGet(&attitudeSettings); // Indicates not to expend cycles on rotation - if (attitudeSettings.BoardRotation.fields.Roll== 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 && + if (attitudeSettings.BoardRotation.fields.Roll == 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Yaw == 0) { rotate = 0; } else { float rotationQuat[4]; const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, - attitudeSettings.BoardRotation.fields.Pitch , - attitudeSettings.BoardRotation.fields.Yaw}; + attitudeSettings.BoardRotation.fields.Pitch, + attitudeSettings.BoardRotation.fields.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index 6b01b8f57..4f2bbffe4 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -70,9 +70,9 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) // On first run initialize estimates to something reasonable if (reinit) { - rateRelayRunning[axis] = false; - relay.Period.data[axis] = 200; - relay.Gain.data[axis] = 0; + rateRelayRunning[axis] = false; + relay.Period.data[axis] = 200; + relay.Gain.data[axis] = 0; accum_sin = 0; accum_cos = 0; @@ -124,9 +124,9 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) accum_cos = 0; if (rateRelayRunning[axis] == false) { - rateRelayRunning[axis] = true; - relay.Period.data[axis] = 200; - relay.Gain.data[axis] = 0; + rateRelayRunning[axis] = true; + relay.Period.data[axis] = 200; + relay.Gain.data[axis] = 0; } else { // Low pass filter each amplitude and period relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index a95b39dd4..7e8ecac87 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -503,7 +503,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll== STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index f2f8421f8..cdc813bfa 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -673,7 +673,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.Throttle = manualControl.Throttle; } - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) { stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 9722fc50d..25eebe6ff 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -125,12 +125,12 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) // Append field // Check if it a named set and creates structures accordingly if (info->fields[n]->numElements > 1) { - if(info->fields[n]->elementNames[0].compare(QString("0")) != 0){ + if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { QString unionTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); - QString unionType = QString("typedef union {\n"); + QString unionType = QString("typedef union {\n"); unionType.append(QString(" %1 data[%2];\n").arg(type).arg(info->fields[n]->numElements)); unionType.append(QString(" struct __attribute__ ((__packed__)) {\n")); - for(int f =0; f < info->fields[n]->elementNames.count(); f++){ + for (int f = 0; f < info->fields[n]->elementNames.count(); f++) { unionType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f])); } unionType.append(QString(" } fields;\n")); @@ -140,10 +140,9 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) fields.append(QString(" %1 %2;\n").arg(unionTypeName) .arg(info->fields[n]->name)); - } else { - fields.append(QString(" %1 %2[%3];\n").arg(type) - .arg(info->fields[n]->name).arg(info->fields[n]->numElements)); + fields.append(QString(" %1 %2[%3];\n").arg(type) + .arg(info->fields[n]->name).arg(info->fields[n]->numElements)); } } else { fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name)); @@ -227,7 +226,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) // Initialize all fields in the array for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { QString optIdentifier; - if(info->fields[n]->elementNames[0].compare(QString("0")) != 0){ + if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { optIdentifier = QString(".data"); } From 8dd3c45ccfefbc70fe7d29616095523f8312e374 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 1 Aug 2013 11:44:12 +0200 Subject: [PATCH 08/11] OP-1058 change some missing occurrencies to .field.XXX notation +review OPReview-552 --- flight/modules/ManualControl/manualcontrol.c | 38 ++++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 42640f542..b1cf5ac12 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -261,10 +261,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } // Check settings, if error raise alarm - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + if (settings.ChannelGroups.fields.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.fields.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.fields.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.fields.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID @@ -281,7 +281,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + && (settings.ChannelGroups.fields.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -296,14 +296,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], - settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], - settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], - settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], - settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + valid_input_detected &= validInputRange(settings.ChannelMin.fields.Throttle, + settings.ChannelMax.fields.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.fields.Roll, + settings.ChannelMax.fields.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.fields.Yaw, + settings.ChannelMax.fields.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.fields.Pitch, + settings.ChannelMax.fields.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -333,21 +333,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -389,7 +389,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); @@ -406,7 +406,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 1 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); @@ -423,7 +423,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 2 - if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); From e39c3f897e9129855c47e62cdfc7da503d36bc73 Mon Sep 17 00:00:00 2001 From: Erik Gustavsson Date: Mon, 26 Aug 2013 19:14:47 +0200 Subject: [PATCH 09/11] Use xTaskGetTickCount() to time long intervals rather than PIOS_DELAY_GetRaw(), because of wrap-around issues. --- flight/modules/StateEstimation/filtercf.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 224cf1953..3c3ba136f 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -47,6 +47,9 @@ #define STACK_REQUIRED 512 +#define CALIBRATION_DELAY_MS 4000 +#define CALIBRATION_DURATION_MS 6000 + // Private types struct data { AttitudeSettingsData attitudeSettings; @@ -274,18 +277,18 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->grot_filtered[0] = 0.0f; this->grot_filtered[1] = 0.0f; this->grot_filtered[2] = 0.0f; - this->timeval = PIOS_DELAY_GetRaw(); - this->starttime = this->timeval; + this->timeval = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing + this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion } - if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 4000000) { + if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { // wait 4 seconds for the user to get his hands off in case the board was just powered this->timeval = PIOS_DELAY_GetRaw(); return 1; - } else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000) { - // For first 7 seconds use accels to get gyro bias + } else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { + // For first 6 seconds use accels to get gyro bias this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.YawBiasRate = 0.23f; From e91bc286676872da52f21fece6af680451713c69 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 1 Sep 2013 12:10:55 +0200 Subject: [PATCH 10/11] OP-1058 Get rid of Unions. They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions. It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays +review OPReview-552 --- flight/libraries/alarms.c | 17 +- flight/modules/AltitudeHold/altitudehold.c | 6 +- flight/modules/Attitude/attitude.c | 28 ++-- flight/modules/Attitude/revolution/attitude.c | 86 +++++----- flight/modules/Battery/battery.c | 8 +- flight/modules/CameraStab/camerastab.c | 39 +++-- .../fixedwingpathfollower.c | 153 +++++++++--------- flight/modules/ManualControl/manualcontrol.c | 151 ++++++++--------- flight/modules/Osd/osdgen/osdgen.c | 22 +-- flight/modules/PathPlanner/pathplanner.c | 44 ++--- flight/modules/Sensors/sensors.c | 46 +++--- flight/modules/Stabilization/relay_tuning.c | 27 ++-- flight/modules/Stabilization/stabilization.c | 87 +++++----- flight/modules/Stabilization/virtualflybar.c | 15 +- flight/modules/StateEstimation/filterekf.c | 77 ++++----- flight/modules/System/systemmod.c | 9 +- flight/modules/TxPID/txpid.c | 93 ++++++----- .../VtolPathFollower/vtolpathfollower.c | 101 ++++++------ flight/pios/inc/pios_struct_helper.h | 9 ++ .../boards/revoproto/firmware/pios_board.c | 2 +- .../flight/uavobjectgeneratorflight.cpp | 42 +++-- 21 files changed, 553 insertions(+), 509 deletions(-) diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 54511cf01..be8e6ec86 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -28,6 +28,7 @@ */ #include +#include #include "inc/alarms.h" // Private constants @@ -74,8 +75,8 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if (alarms.Alarm.data[alarm] != severity) { - alarms.Alarm.data[alarm] = severity; + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) { + cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity; SystemAlarmsSet(&alarms); } @@ -109,10 +110,10 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm, // Read alarm and update its severity only if it was changed SystemAlarmsGet(&alarms); - if (alarms.Alarm.data[alarm] != severity) { - alarms.ExtendedAlarmStatus.data[alarm] = status; - alarms.ExtendedAlarmSubStatus.data[alarm] = subStatus; - alarms.Alarm.data[alarm] = severity; + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) { + cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status; + cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus; + cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity; SystemAlarmsSet(&alarms); } @@ -137,7 +138,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm) // Read alarm SystemAlarmsGet(&alarms); - return alarms.Alarm.data[alarm]; + return cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm]; } /** @@ -229,7 +230,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) // Go through alarms and check if any are of the given severity or higher for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { - if (alarms.Alarm.data[n] >= severity) { + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) { xSemaphoreGiveRecursive(lock); return 1; } diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 7cf41feeb..f19759524 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -392,9 +392,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters) // shutdown motors stabilizationDesired.Throttle = -1; } - stabilizationDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabilizationDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index d3c88e009..c64bc6272 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -609,17 +609,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; - accelbias[0] = attitudeSettings.AccelBias.fields.X; - accelbias[1] = attitudeSettings.AccelBias.fields.Y; - accelbias[2] = attitudeSettings.AccelBias.fields.Z; + accelbias[0] = attitudeSettings.AccelBias.X; + accelbias[1] = attitudeSettings.AccelBias.Y; + accelbias[2] = attitudeSettings.AccelBias.Z; - gyro_correct_int[0] = attitudeSettings.GyroBias.fields.X / 100.0f; - gyro_correct_int[1] = attitudeSettings.GyroBias.fields.Y / 100.0f; - gyro_correct_int[2] = attitudeSettings.GyroBias.fields.Z / 100.0f; + gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f; + gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f; + gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f; // Indicates not to expend cycles on rotation - if (attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Roll == 0 && - attitudeSettings.BoardRotation.fields.Yaw == 0) { + if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 && + attitudeSettings.BoardRotation.Yaw == 0) { rotate = 0; // Shouldn't be used but to be safe @@ -627,9 +627,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) Quaternion2R(rotationQuat, R); } else { float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, - attitudeSettings.BoardRotation.fields.Pitch, - attitudeSettings.BoardRotation.fields.Yaw }; + const float rpy[3] = { attitudeSettings.BoardRotation.Roll, + attitudeSettings.BoardRotation.Pitch, + attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; @@ -643,10 +643,10 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) trim_requested = true; } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { trim_requested = false; - attitudeSettings.AccelBias.fields.X = trim_accels[0] / trim_samples; - attitudeSettings.AccelBias.fields.Y = trim_accels[1] / trim_samples; + attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples; + attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples; // Z should average -grav - attitudeSettings.AccelBias.fields.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; + attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; AttitudeSettingsSet(&attitudeSettings); } else { diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index 9a9a0449b..ee616e44d 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -49,7 +49,7 @@ */ #include - +#include #include "attitude.h" #include "accelsensor.h" #include "accelstate.h" @@ -834,12 +834,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) value_error = true; } - if (invalid_var(ekfConfiguration.R.fields.GPSPosNorth) || - invalid_var(ekfConfiguration.R.fields.GPSPosEast) || - invalid_var(ekfConfiguration.R.fields.GPSPosDown) || - invalid_var(ekfConfiguration.R.fields.GPSVelNorth) || - invalid_var(ekfConfiguration.R.fields.GPSVelEast) || - invalid_var(ekfConfiguration.R.fields.GPSVelDown)) { + 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; } @@ -892,23 +892,23 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if (init_stage == 0) { // Reset the INS algorithm INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R.fields.MagX, - ekfConfiguration.R.fields.MagY, - ekfConfiguration.R.fields.MagZ } + INSSetMagVar((float[3]) { ekfConfiguration.R.MagX, + ekfConfiguration.R.MagY, + ekfConfiguration.R.MagZ } ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q.fields.AccelX, - ekfConfiguration.Q.fields.AccelY, - ekfConfiguration.Q.fields.AccelZ } + INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX, + ekfConfiguration.Q.AccelY, + ekfConfiguration.Q.AccelZ } ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q.fields.GyroX, - ekfConfiguration.Q.fields.GyroY, - ekfConfiguration.Q.fields.GyroZ } + INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX, + ekfConfiguration.Q.GyroY, + ekfConfiguration.Q.GyroZ } ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.fields.GyroDriftX, - ekfConfiguration.Q.fields.GyroDriftY, - ekfConfiguration.Q.fields.GyroDriftZ } + INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX, + ekfConfiguration.Q.GyroDriftY, + ekfConfiguration.Q.GyroDriftZ } ); - INSSetBaroVar(ekfConfiguration.R.fields.BaroZ); + INSSetBaroVar(ekfConfiguration.R.BaroZ); // Initialize the gyro bias float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; @@ -966,7 +966,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; INSSetState(pos, zeros, q, zeros, zeros); - INSResetP(ekfConfiguration.P.data); + INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)); } else { // Run prediction a bit before any corrections @@ -1028,12 +1028,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.R.fields.GPSPosNorth, - ekfConfiguration.R.fields.GPSPosEast, - ekfConfiguration.R.fields.GPSPosDown }, - (float[3]) { ekfConfiguration.R.fields.GPSVelNorth, - ekfConfiguration.R.fields.GPSVelEast, - ekfConfiguration.R.fields.GPSVelDown } + 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; @@ -1051,12 +1051,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) (1.0f - BARO_OFFSET_LOWPASS_ALPHA) * (-NED[2] - baroData.Altitude); } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, - ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, - ekfConfiguration.FakeR.fields.FakeGPSVelIndoor } + 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; @@ -1084,12 +1084,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) 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.fields.FakeGPSPosIndoor, - ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed } + 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]; @@ -1130,7 +1130,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(vardata.P.data); + INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); EKFStateVarianceSet(&vardata); return 0; @@ -1180,17 +1180,17 @@ static void settingsUpdatedCb(UAVObjEvent *ev) EKFConfigurationGet(&ekfConfiguration); int t; for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(ekfConfiguration.P.data[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(ekfConfiguration.Q.data[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(ekfConfiguration.R.data[t])) { + if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) { error = true; } } diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index f52e0298c..b74094add 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -135,13 +135,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // calculate the battery parameters if (voltageADCPin >= 0) { - flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.fields.VoltageZero) * batterySettings.SensorCalibrations.fields.VoltageFactor; // in Volts + flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts } else { flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >= 0) { - flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.fields.CurrentZero) * batterySettings.SensorCalibrations.fields.CurrentFactor; // in Amps + flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.CurrentZero) * batterySettings.SensorCalibrations.CurrentFactor; // in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps } @@ -184,9 +184,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Alarm * batterySettings.NbCells) { + if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); - } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Warning * batterySettings.NbCells) { + } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * batterySettings.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index d616eb935..6493022b9 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -52,6 +52,7 @@ #include "camerastabsettings.h" #include "cameradesired.h" #include "hwsettings.h" +#include // // Configuration @@ -171,17 +172,21 @@ static void attitudeUpdated(UAVObjEvent *ev) // process axes for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { // read and process control input - if (cameraStab.Input.data[i] != CAMERASTABSETTINGS_INPUT_NONE) { - if (AccessoryDesiredInstGet(cameraStab.Input.data[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + if (cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] != CAMERASTABSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] - + CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { float input_rate; - switch (cameraStab.StabilizationMode.data[i]) { + switch (cast_struct_to_array(cameraStab.StabilizationMode, cameraStab.StabilizationMode.Roll)[i]) { case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: - csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange.data[i]; + csd->inputs[i] = accessory.AccessoryVal * + cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]; break; case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: - input_rate = accessory.AccessoryVal * cameraStab.InputRate.data[i]; + input_rate = accessory.AccessoryVal * + cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i]; if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { - csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange.data[i]); + csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, + cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]); } break; default: @@ -208,14 +213,14 @@ static void attitudeUpdated(UAVObjEvent *ev) } #ifdef USE_GIMBAL_LPF - if (cameraStab.ResponseTime.data) { - float rt = (float)cameraStab.ResponseTime.data[i]; + if (cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i]) { + float rt = (float)cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i]; attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); } #endif #ifdef USE_GIMBAL_FF - if (cameraStab.FeedForward.data[i]) { + if (cast_struct_to_array(cameraStab.FeedForward, cameraStab.FeedForward.Roll)[i]) { applyFeedForward(i, dT_millis, &attitude, &cameraStab); } #endif @@ -223,7 +228,7 @@ static void attitudeUpdated(UAVObjEvent *ev) // bounding for elevon mixing occurs on the unmixed output // to limit the range of the mixed output you must limit the range // of both the unmixed pitch and unmixed roll - float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange.data[i], 1.0f); + float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f); // set output channels switch (i) { @@ -298,16 +303,16 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta if (index == CAMERASTABSETTINGS_INPUT_ROLL) { float pitch; AttitudeStatePitchGet(&pitch); - gimbalTypeCorrection = (cameraStab->OutputRange.fields.Pitch - fabsf(pitch)) - / cameraStab->OutputRange.fields.Pitch; + gimbalTypeCorrection = (cameraStab->OutputRange.Pitch - fabsf(pitch)) + / cameraStab->OutputRange.Pitch; } break; case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: if (index == CAMERASTABSETTINGS_INPUT_PITCH) { float roll; AttitudeStateRollGet(&roll); - gimbalTypeCorrection = (cameraStab->OutputRange.fields.Roll - fabsf(roll)) - / cameraStab->OutputRange.fields.Roll; + gimbalTypeCorrection = (cameraStab->OutputRange.Roll - fabsf(roll)) + / cameraStab->OutputRange.Roll; } break; default: @@ -316,11 +321,13 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta // apply feed forward float accumulator = csd->ffFilterAccumulator[index]; - accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward.data[index] * gimbalTypeCorrection; + accumulator += (*attitude - csd->ffLastAttitude[index]) * + (float)cast_struct_to_array(cameraStab->FeedForward, cameraStab->FeedForward.Roll)[index] * gimbalTypeCorrection; csd->ffLastAttitude[index] = *attitude; *attitude += accumulator; - float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime.data[index] : cameraStab->DecelTime.data[index]) / dT_millis; + float filter = (float)((accumulator > 0.0f) ? cast_struct_to_array(cameraStab->AccelTime, cameraStab->AccelTime.Roll)[index] : + cast_struct_to_array(cameraStab->DecelTime, cameraStab->DecelTime.Roll)[index]) / dT_millis; if (filter < 1.0f) { filter = 1.0f; } diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 00681358c..fe51f0eca 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -62,6 +62,7 @@ #include "velocitydesired.h" #include "velocitystate.h" #include "taskinfo.h" +#include #include "paths.h" #include "CoordinateConversions.h" @@ -261,7 +262,9 @@ static void updatePathVelocity() positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; struct path_status progress; - path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); + 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; float altitudeSetpoint; @@ -271,7 +274,7 @@ static void updatePathVelocity() case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT: groundspeed = pathDesired.EndingVelocity; - altitudeSetpoint = pathDesired.End.fields.Down; + altitudeSetpoint = pathDesired.End.Down; break; case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT: @@ -280,7 +283,7 @@ static void updatePathVelocity() default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1); - altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * + altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1); break; } @@ -358,9 +361,9 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; StabilizationDesiredSet(&stabDesired); } @@ -444,29 +447,29 @@ static uint8_t updateFixedDesiredAttitude() descentspeedError = descentspeedDesired - velocityState.Down; // Error condition: plane too slow or too fast - fixedwingpathfollowerStatus.Errors.fields.Highspeed = 0; - fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 0; - if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.fields.Overspeed) { - fixedwingpathfollowerStatus.Errors.fields.Overspeed = 1; + 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.fields.Highspeed) { - fixedwingpathfollowerStatus.Errors.fields.Highspeed = 1; + if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) { + fixedwingpathfollowerStatus.Errors.Highspeed = 1; result = 0; } - if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.fields.Lowspeed) { - fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; + if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) { + fixedwingpathfollowerStatus.Errors.Lowspeed = 1; result = 0; } - if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.fields.Stallspeed) { - fixedwingpathfollowerStatus.Errors.fields.Stallspeed = 1; + if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.Stallspeed) { + fixedwingpathfollowerStatus.Errors.Stallspeed = 1; result = 0; } if (indicatedAirspeedState < 1e-6f) { // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // also we cannot handle planes flying backwards, lets just wait until the nose drops - fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; + fixedwingpathfollowerStatus.Errors.Lowspeed = 1; return 0; } @@ -474,53 +477,53 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired throttle command */ // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. - if (fixedwingpathfollowerSettings.PowerPI.fields.Ki > 0) { + if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) { powerIntegral = bound(powerIntegral + -descentspeedError * dT, - -fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki, - fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki + -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, + fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki ) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } else { powerIntegral = 0; } // Compute the cross feed from vertical speed to pitch, with saturation float speedErrorToPowerCommandComponent = bound( - (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Kp, - -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max, - fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max + (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp, + -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max, + fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max ); // Compute final throttle response - powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.fields.Kp + - powerIntegral * fixedwingpathfollowerSettings.PowerPI.fields.Ki + + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp + + powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki + speedErrorToPowerCommandComponent; // Output internal state to telemetry - fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError; - fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral; - fixedwingpathfollowerStatus.Command.fields.Power = powerCommand; + fixedwingpathfollowerStatus.Error.Power = descentspeedError; + fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral; + fixedwingpathfollowerStatus.Command.Power = powerCommand; // set throttle - stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand, - fixedwingpathfollowerSettings.ThrottleLimit.fields.Min, - fixedwingpathfollowerSettings.ThrottleLimit.fields.Max); + stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand, + fixedwingpathfollowerSettings.ThrottleLimit.Min, + fixedwingpathfollowerSettings.ThrottleLimit.Max); // Error condition: plane cannot hold altitude at current speed. - fixedwingpathfollowerStatus.Errors.fields.Lowpower = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Max && // throttle at maximum + fixedwingpathfollowerStatus.Errors.Lowpower = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError > 0 && // we are too slow already - fixedwingpathfollowerSettings.Safetymargins.fields.Lowpower > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.fields.Lowpower = 1; + fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.Lowpower = 1; result = 0; } // Error condition: plane keeps climbing despite minimum throttle (opposite of above) - fixedwingpathfollowerStatus.Errors.fields.Highpower = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Min && // throttle at minimum + fixedwingpathfollowerStatus.Errors.Highpower = 0; + if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum velocityState.Down < 0 && // we ARE going up descentspeedDesired > 0 && // we WANT to go down airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.fields.Highpower = 1; + fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.Highpower = 1; result = 0; } @@ -529,40 +532,40 @@ static uint8_t updateFixedDesiredAttitude() * Compute desired pitch command */ - if (fixedwingpathfollowerSettings.SpeedPI.fields.Ki > 0) { + if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) { // Integrate with saturation airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, - -fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki, - fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki); + -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 = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Kp, - -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max, - fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max + float verticalSpeedToPitchCommandComponent = bound(-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.fields.Kp - + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki + pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp + + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki ) + verticalSpeedToPitchCommandComponent; - fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError; - fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt; - fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand; + fixedwingpathfollowerStatus.Error.Speed = airspeedError; + fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt; + fixedwingpathfollowerStatus.Command.Speed = pitchCommand; - stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand, - fixedwingpathfollowerSettings.PitchLimit.fields.Min, - fixedwingpathfollowerSettings.PitchLimit.fields.Max); + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand, + fixedwingpathfollowerSettings.PitchLimit.Min, + fixedwingpathfollowerSettings.PitchLimit.Max); // Error condition: high speed dive - fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 0; - if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.fields.Max && // pitch demand is full up + fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0; + if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError < 0 && // we are too fast already - fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on - fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1; + fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1; result = 0; } @@ -579,12 +582,12 @@ static uint8_t updateFixedDesiredAttitude() headingError -= 360.0f; } // Error condition: wind speed is higher than airspeed. We are forced backwards! - fixedwingpathfollowerStatus.Errors.fields.Wind = 0; - if ((headingError > fixedwingpathfollowerSettings.Safetymargins.fields.Wind || - headingError < -fixedwingpathfollowerSettings.Safetymargins.fields.Wind) && - fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on + fixedwingpathfollowerStatus.Errors.Wind = 0; + if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind || + headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) && + fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on // we are flying backwards - fixedwingpathfollowerStatus.Errors.fields.Wind = 1; + fixedwingpathfollowerStatus.Errors.Wind = 1; result = 0; } @@ -608,20 +611,20 @@ static uint8_t updateFixedDesiredAttitude() courseError -= 360.0f; } - courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.fields.Ki, - -fixedwingpathfollowerSettings.CoursePI.fields.ILimit, - fixedwingpathfollowerSettings.CoursePI.fields.ILimit); - courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp + + courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki, + -fixedwingpathfollowerSettings.CoursePI.ILimit, + fixedwingpathfollowerSettings.CoursePI.ILimit); + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp + courseIntegral); - fixedwingpathfollowerStatus.Error.fields.Course = courseError; - fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral; - fixedwingpathfollowerStatus.Command.fields.Course = courseCommand; + fixedwingpathfollowerStatus.Error.Course = courseError; + fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral; + fixedwingpathfollowerStatus.Command.Course = courseCommand; - stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral + + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral + courseCommand, - fixedwingpathfollowerSettings.RollLimit.fields.Min, - fixedwingpathfollowerSettings.RollLimit.fields.Max); + fixedwingpathfollowerSettings.RollLimit.Min, + fixedwingpathfollowerSettings.RollLimit.Max); // TODO: find a check to determine loss of directional control. Likely needs some check of derivative @@ -633,9 +636,9 @@ static uint8_t updateFixedDesiredAttitude() stabDesired.Yaw = 0; - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; StabilizationDesiredSet(&stabDesired); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b1cf5ac12..db1ae3cce 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -34,7 +34,7 @@ */ #include - +#include #include "accessorydesired.h" #include "actuatordesired.h" #include "altitudeholddesired.h" @@ -245,10 +245,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters) for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { extern uint32_t pios_rcvr_group_map[]; - if (settings.ChannelGroups.data[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { cmd.Channel[n] = PIOS_RCVR_INVALID; } else { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups.data[n]], settings.ChannelNumber.data[n]); + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ + cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], + cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); } // If a channel has timed out this is not valid data and we shouldn't update anything @@ -256,15 +258,18 @@ static void manualControlTask(__attribute__((unused)) void *parameters) if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { valid_input_detected = false; } else { - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax.data[n], settings.ChannelMin.data[n], settings.ChannelNeutral.data[n]); + scaledChannel[n] = scaleChannel(cmd.Channel[n], + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], + cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); } } // Check settings, if error raise alarm - if (settings.ChannelGroups.fields.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.fields.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.fields.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.fields.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID @@ -281,7 +286,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups.fields.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); @@ -296,14 +301,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin.fields.Throttle, - settings.ChannelMax.fields.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin.fields.Roll, - settings.ChannelMax.fields.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin.fields.Yaw, - settings.ChannelMax.fields.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin.fields.Pitch, - settings.ChannelMax.fields.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, + settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.Roll, + settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.Yaw, + settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.Pitch, + settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -333,21 +338,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 - if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 - if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -389,7 +394,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 - if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); @@ -406,7 +411,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 1 - if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); @@ -423,7 +428,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } // Set Accessory 2 - if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); @@ -453,8 +458,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters) if (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, cmd.Channel, - settings.ChannelMin.data, - settings.ChannelMax.data, + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), NELEMENTS(cmd.Channel)); } #endif /* PIOS_INCLUDE_USB_RCTX */ @@ -671,13 +676,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings.data; + stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings.data; + stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll); break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings.data; + stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll); break; default: // Major error, this should not occur because only enter this block when one of these is true @@ -686,44 +691,44 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode.fields.Roll = stab_settings[0]; - stabilization.StabilizationMode.fields.Pitch = stab_settings[1]; - stabilization.StabilizationMode.fields.Yaw = stab_settings[2]; + stabilization.StabilizationMode.Roll = stab_settings[0]; + stabilization.StabilizationMode.Pitch = stab_settings[1]; + stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate.fields.Roll : + cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Roll * stabSettings.ManualRate.fields.Roll : + cmd->Roll * stabSettings.ManualRate.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.fields.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate.fields.Pitch : + cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate.fields.Pitch : + cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - cmd->Pitch * stabSettings.ManualRate.fields.Pitch : + cmd->Pitch * stabSettings.ManualRate.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate.fields.Yaw : + cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; @@ -754,14 +759,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start.fields.North = 0; - pathDesired.Start.fields.East = 0; - pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.fields.North = 0; - pathDesired.End.fields.East = 0; - pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.North = 0; + pathDesired.Start.East = 0; + pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.North = 0; + pathDesired.End.East = 0; + pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } else if (changed) { @@ -771,14 +776,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start.fields.North = positionState.North; - pathDesired.Start.fields.East = positionState.East; - pathDesired.Start.fields.Down = positionState.Down; - pathDesired.End.fields.North = positionState.North; - pathDesired.End.fields.East = positionState.East; - pathDesired.End.fields.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. @@ -813,17 +818,17 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData * PathDesiredGet(&pathDesired); if (changed) { // After not being in this mode for a while init at current height - pathDesired.Start.fields.North = positionState.North; - pathDesired.Start.fields.East = positionState.East; - pathDesired.Start.fields.Down = positionState.Down; - pathDesired.End.fields.North = positionState.North; - pathDesired.End.fields.East = positionState.East; - pathDesired.End.fields.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End.fields.Down = positionState.Down + 5; + pathDesired.End.Down = positionState.Down + 5; PathDesiredSet(&pathDesired); } @@ -875,7 +880,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.fields.Yaw; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; float currentDown; PositionStateDownGet(¤tDown); @@ -974,7 +979,7 @@ static bool okToArm(void) // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (alarms.Alarm.data[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; } @@ -1007,7 +1012,7 @@ static bool forcedDisArm(void) SystemAlarmsGet(&alarms); - if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { + if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { return true; } return false; @@ -1246,8 +1251,8 @@ static void applyDeadband(float *value, float deadband) */ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) { - if (settings->ResponseTime.data[channel]) { - float rt = (float)settings->ResponseTime.data[channel]; + if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) { + float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]; inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); *value = inputFiltered[channel]; } diff --git a/flight/modules/Osd/osdgen/osdgen.c b/flight/modules/Osd/osdgen/osdgen.c index fa313a9fd..5a5083c08 100644 --- a/flight/modules/Osd/osdgen/osdgen.c +++ b/flight/modules/Osd/osdgen/osdgen.c @@ -2219,8 +2219,8 @@ void updateGraphics() /* Draw Attitude Indicator */ if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { - drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.fields.X), - APPLY_VDEADBAND(OsdSettings.AttitudeSetup.fields.Y), attitude.Pitch, attitude.Roll, 96); + drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.X), + APPLY_VDEADBAND(OsdSettings.AttitudeSetup.Y), attitude.Pitch, attitude.Roll, 96); } // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); // printText16( 60, 12,"Hello OP-OSD"); @@ -2239,7 +2239,7 @@ void updateGraphics() /* Print RTC time */ if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { - printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.fields.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.fields.Y)); + printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.Y)); } /* Print Number of detected video Lines */ @@ -2292,22 +2292,22 @@ void updateGraphics() // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); // Draw airspeed (left side.) if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { - hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.fields.X), - APPLY_VDEADBAND(OsdSettings.SpeedSetup.fields.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); + hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.X), + APPLY_VDEADBAND(OsdSettings.SpeedSetup.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); } // Draw altimeter (right side.) if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { - hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.fields.X), - APPLY_VDEADBAND(OsdSettings.AltitudeSetup.fields.Y), 100, 20, 100, 7, 12, 15, 500, 0); + hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.X), + APPLY_VDEADBAND(OsdSettings.AltitudeSetup.Y), 100, 20, 100, 7, 12, 15, 500, 0); } // Draw compass. if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { if (attitude.Yaw < 0) { - hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), - APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); + hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X), + APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0); } else { - hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), - APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); + hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X), + APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0); } } } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index da5ef0225..837e78608 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -41,7 +41,7 @@ #include "waypoint.h" #include "waypointactive.h" #include "taskinfo.h" - +#include #include "paths.h" // Private constants @@ -227,9 +227,9 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) WaypointInstGet(waypointActiveData.Index, &waypointData); PathActionInstGet(waypointData.Action, &pathActionData); - pathDesired.End.fields.North = waypointData.Position.fields.North; - pathDesired.End.fields.East = waypointData.Position.fields.East; - pathDesired.End.fields.Down = waypointData.Position.fields.Down; + pathDesired.End.North = waypointData.Position.North; + pathDesired.End.East = waypointData.Position.East; + pathDesired.End.Down = waypointData.Position.Down; pathDesired.EndingVelocity = waypointData.Velocity; pathDesired.Mode = pathActionData.Mode; pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; @@ -246,19 +246,19 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev) /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ - pathDesired.Start.fields.North = positionState.North; - pathDesired.Start.fields.East = positionState.East; - pathDesired.Start.fields.Down = positionState.Down; - pathDesired.StartingVelocity = pathDesired.EndingVelocity; + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = pathDesired.EndingVelocity; } else { // Get previous waypoint as start point WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - pathDesired.Start.fields.North = waypointPrev.Position.fields.North; - pathDesired.Start.fields.East = waypointPrev.Position.fields.East; - pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down; - pathDesired.StartingVelocity = waypointPrev.Velocity; + pathDesired.Start.North = waypointPrev.Position.North; + pathDesired.Start.East = waypointPrev.Position.East; + pathDesired.Start.Down = waypointPrev.Position.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); } @@ -369,12 +369,12 @@ static uint8_t conditionDistanceToTarget() PositionStateGet(&positionState); if (pathAction.ConditionParameters[1] > 0.5f) { - distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2) - + powf(waypoint.Position.fields.East - positionState.East, 2) - + powf(waypoint.Position.fields.Down - positionState.Down, 2)); + distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2) + + powf(waypoint.Position.East - positionState.East, 2) + + powf(waypoint.Position.Down - positionState.Down, 2)); } else { - distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2) - + powf(waypoint.Position.fields.East - positionState.East, 2)); + distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2) + + powf(waypoint.Position.East - positionState.East, 2)); } if (distance <= pathAction.ConditionParameters[0]) { @@ -400,7 +400,9 @@ static uint8_t conditionLegRemaining() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { return true; } @@ -423,7 +425,9 @@ static uint8_t conditionBelowError() float cur[3] = { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); + path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North), + cast_struct_to_array(pathDesired.End, pathDesired.End.North), + cur, &progress, pathDesired.Mode); if (progress.error <= pathAction.ConditionParameters[0]) { return true; } @@ -492,7 +496,7 @@ static uint8_t conditionPointingTowardsNext() WaypointData nextWaypoint; WaypointInstGet(nextWaypointId, &nextWaypoint); - float angle1 = atan2f((nextWaypoint.Position.fields.North - waypoint.Position.fields.North), (nextWaypoint.Position.fields.East - waypoint.Position.fields.East)); + float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East)); VelocityStateData velocity; VelocityStateGet(&velocity); diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 77660a63c..cc99a0e76 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -422,38 +422,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { RevoCalibrationGet(&cal); - mag_bias[0] = cal.mag_bias.fields.X; - mag_bias[1] = cal.mag_bias.fields.Y; - mag_bias[2] = cal.mag_bias.fields.Z; - mag_scale[0] = cal.mag_scale.fields.X; - mag_scale[1] = cal.mag_scale.fields.Y; - mag_scale[2] = cal.mag_scale.fields.Z; - accel_bias[0] = cal.accel_bias.fields.X; - accel_bias[1] = cal.accel_bias.fields.Y; - accel_bias[2] = cal.accel_bias.fields.Z; - accel_scale[0] = cal.accel_scale.fields.X; - accel_scale[1] = cal.accel_scale.fields.Y; - accel_scale[2] = cal.accel_scale.fields.Z; - gyro_staticbias[0] = cal.gyro_bias.fields.X; - gyro_staticbias[1] = cal.gyro_bias.fields.Y; - gyro_staticbias[2] = cal.gyro_bias.fields.Z; - gyro_scale[0] = cal.gyro_scale.fields.X; - gyro_scale[1] = cal.gyro_scale.fields.Y; - gyro_scale[2] = cal.gyro_scale.fields.Z; + mag_bias[0] = cal.mag_bias.X; + mag_bias[1] = cal.mag_bias.Y; + mag_bias[2] = cal.mag_bias.Z; + mag_scale[0] = cal.mag_scale.X; + mag_scale[1] = cal.mag_scale.Y; + mag_scale[2] = cal.mag_scale.Z; + accel_bias[0] = cal.accel_bias.X; + accel_bias[1] = cal.accel_bias.Y; + accel_bias[2] = cal.accel_bias.Z; + accel_scale[0] = cal.accel_scale.X; + accel_scale[1] = cal.accel_scale.Y; + accel_scale[2] = cal.accel_scale.Z; + gyro_staticbias[0] = cal.gyro_bias.X; + gyro_staticbias[1] = cal.gyro_bias.Y; + gyro_staticbias[2] = cal.gyro_bias.Z; + gyro_scale[0] = cal.gyro_scale.X; + gyro_scale[1] = cal.gyro_scale.Y; + gyro_scale[2] = cal.gyro_scale.Z; AttitudeSettingsData attitudeSettings; AttitudeSettingsGet(&attitudeSettings); // Indicates not to expend cycles on rotation - if (attitudeSettings.BoardRotation.fields.Roll == 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 && - attitudeSettings.BoardRotation.fields.Yaw == 0) { + if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 && + attitudeSettings.BoardRotation.Yaw == 0) { rotate = 0; } else { float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, - attitudeSettings.BoardRotation.fields.Pitch, - attitudeSettings.BoardRotation.fields.Yaw }; + const float rpy[3] = { attitudeSettings.BoardRotation.Roll, + attitudeSettings.BoardRotation.Pitch, + attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; diff --git a/flight/modules/Stabilization/relay_tuning.c b/flight/modules/Stabilization/relay_tuning.c index 4f2bbffe4..6d5f2c36d 100644 --- a/flight/modules/Stabilization/relay_tuning.c +++ b/flight/modules/Stabilization/relay_tuning.c @@ -32,6 +32,7 @@ */ #include "openpilot.h" +#include #include "stabilization.h" #include "relaytuning.h" #include "relaytuningsettings.h" @@ -70,9 +71,9 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) // On first run initialize estimates to something reasonable if (reinit) { - rateRelayRunning[axis] = false; - relay.Period.data[axis] = 200; - relay.Gain.data[axis] = 0; + rateRelayRunning[axis] = false; + cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200; + cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0; accum_sin = 0; accum_cos = 0; @@ -95,14 +96,14 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) /**** The code below here is to estimate the properties of the oscillation ****/ // Make sure the period can't go below limit - if (relay.Period.data[axis] < DEGLITCH_TIME) { - relay.Period.data[axis] = DEGLITCH_TIME; + if (cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] < DEGLITCH_TIME) { + cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = DEGLITCH_TIME; } // Project the error onto a sine and cosine of the same frequency // to accumulate the average amplitude int32_t dT = thisTime - lastHighTime; - float phase = ((float)360 * (float)dT) / relay.Period.data[axis]; + float phase = ((float)360 * (float)dT) / cast_struct_to_array(relay.Period, relay.Period.Roll)[axis]; if (phase >= 360) { phase = 0; } @@ -124,13 +125,17 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) accum_cos = 0; if (rateRelayRunning[axis] == false) { - rateRelayRunning[axis] = true; - relay.Period.data[axis] = 200; - relay.Gain.data[axis] = 0; + rateRelayRunning[axis] = true; + cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200; + cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0; } else { // Low pass filter each amplitude and period - relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); - relay.Period.data[axis] = relay.Period.data[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); + cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = + cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] * + AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); + cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = + cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] * + PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); } lastHighTime = thisTime; high = true; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ef9daab21..3ca9f3afc 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -32,7 +32,7 @@ */ #include - +#include #include "stabilization.h" #include "stabilizationsettings.h" #include "actuatordesired.h" @@ -201,11 +201,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } else { // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2 speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed); - if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) { - speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]; + if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) { + speedScaleFactor = settings.ScaleToAirspeedLimits.Min; } - if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) { - speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]; + if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) { + speedScaleFactor = settings.ScaleToAirspeedLimits.Max; } } #else @@ -220,19 +220,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) float local_error[3]; // Essentially zero errors for anything in rate or none - if (stabDesired.StabilizationMode.fields.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[0] = stabDesired.Roll; } else { rpy_desired[0] = attitudeState.Roll; } - if (stabDesired.StabilizationMode.fields.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[1] = stabDesired.Pitch; } else { rpy_desired[1] = attitudeState.Pitch; } - if (stabDesired.StabilizationMode.fields.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { + if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { rpy_desired[2] = stabDesired.Yaw; } else { rpy_desired[2] = attitudeState.Yaw; @@ -276,18 +276,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Run the selected stabilization algorithm on each axis: for (uint8_t i = 0; i < MAX_AXES; i++) { // Check whether this axis mode needs to be reinitialized - bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]); - previous_mode[i] = stabDesired.StabilizationMode.data[i]; + bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]); + previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]; // Apply the selected control law - switch (stabDesired.StabilizationMode.data[i]) { + switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: if (reinit) { pids[PID_RATE_ROLL + i].iAccumulator = 0; } // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); + rateDesiredAxis[i] = + bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); @@ -302,8 +303,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); + rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], + cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); @@ -354,7 +356,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate.data[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], + cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); @@ -363,7 +366,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: // Store to rate desired variable for storing to UAVO - rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); + rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], + cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); @@ -378,7 +382,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Compute the outer loop like attitude mode rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], + cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]); // Run the relay controller which also estimates the oscillation parameters stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); @@ -484,37 +489,37 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) StabilizationSettingsGet(&settings); // Set the roll rate PID constants - pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp, - settings.RollRatePID.fields.Ki, - pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd, - pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit); + pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp, + settings.RollRatePID.Ki, + pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd, + pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit); // Set the pitch rate PID constants - pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp, - pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki, - pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd, - pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit); + pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp, + pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki, + pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd, + pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit); // Set the yaw rate PID constants - pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp, - pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki, - pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd, - pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit); + pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp, + pids[PID_RATE_YAW].i = settings.YawRatePID.Ki, + pids[PID_RATE_YAW].d = settings.YawRatePID.Kd, + pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit); // Set the roll attitude PI constants - pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp, - settings.RollPI.fields.Ki, 0, - pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit); + pid_configure(&pids[PID_ROLL], settings.RollPI.Kp, + settings.RollPI.Ki, 0, + pids[PID_ROLL].iLim = settings.RollPI.ILimit); // Set the pitch attitude PI constants - pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp, - pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0, - settings.PitchPI.fields.ILimit); + pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp, + pids[PID_PITCH].i = settings.PitchPI.Ki, 0, + settings.PitchPI.ILimit); // Set the yaw attitude PI constants - pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp, - settings.YawPI.fields.Ki, 0, - settings.YawPI.fields.ILimit); + pid_configure(&pids[PID_YAW], settings.YawPI.Kp, + settings.YawPI.Ki, 0, + settings.YawPI.ILimit); // Set up the derivative term pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); @@ -531,9 +536,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant diff --git a/flight/modules/Stabilization/virtualflybar.c b/flight/modules/Stabilization/virtualflybar.c index 1e7c7baab..211b7b864 100644 --- a/flight/modules/Stabilization/virtualflybar.c +++ b/flight/modules/Stabilization/virtualflybar.c @@ -32,6 +32,7 @@ #include "openpilot.h" #include +#include #include "stabilization.h" #include "stabilizationsettings.h" @@ -64,23 +65,23 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float // Get the settings for the correct axis switch (axis) { case ROLL: - kp = settings->VbarRollPI.fields.Kp; - ki = settings->VbarRollPI.fields.Ki; + kp = settings->VbarRollPI.Kp; + ki = settings->VbarRollPI.Ki; break; case PITCH: - kp = settings->VbarPitchPI.fields.Kp; - ki = settings->VbarPitchPI.fields.Ki;; + kp = settings->VbarPitchPI.Kp; + ki = settings->VbarPitchPI.Ki;; break; case YAW: - kp = settings->VbarYawPI.fields.Kp; - ki = settings->VbarYawPI.fields.Ki; + kp = settings->VbarYawPI.Kp; + ki = settings->VbarYawPI.Ki; break; default: PIOS_DEBUG_Assert(0); } // Command signal is composed of stick input added to the gyro and virtual flybar - *output = command * settings->VbarSensitivity.data[axis] - + *output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] - gyro_gain * (vbar_integral[axis] * ki + gyro * kp); return 0; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 2de51506d..a141e4d8f 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -31,6 +31,7 @@ */ #include "inc/stateestimation.h" +#include #include #include @@ -163,17 +164,17 @@ static int32_t maininit(stateFilter *self) int t; // plausibility check for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.P.data[t])) { + if (invalid_var(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.Q.data[t])) { + if (invalid_var(cast_struct_to_array(this->ekfConfiguration.Q, this->ekfConfiguration.Q.AccelX)[t])) { return 2; } } for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(this->ekfConfiguration.R.data[t])) { + if (invalid_var(cast_struct_to_array(this->ekfConfiguration.R, this->ekfConfiguration.R.BaroZ)[t])) { return 2; } } @@ -242,23 +243,23 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSGPSInit(); // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2 float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2]; - INSSetMagVar((float[3]) { this->ekfConfiguration.R.fields.MagX / Be2, - this->ekfConfiguration.R.fields.MagY / Be2, - this->ekfConfiguration.R.fields.MagZ / Be2 } + INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX / Be2, + this->ekfConfiguration.R.MagY / Be2, + this->ekfConfiguration.R.MagZ / Be2 } ); - INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.fields.AccelX, - this->ekfConfiguration.Q.fields.AccelY, - this->ekfConfiguration.Q.fields.AccelZ } + INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX, + this->ekfConfiguration.Q.AccelY, + this->ekfConfiguration.Q.AccelZ } ); - INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.fields.GyroX, - this->ekfConfiguration.Q.fields.GyroY, - this->ekfConfiguration.Q.fields.GyroZ } + INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.GyroX, + this->ekfConfiguration.Q.GyroY, + this->ekfConfiguration.Q.GyroZ } ); - INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.fields.GyroDriftX, - this->ekfConfiguration.Q.fields.GyroDriftY, - this->ekfConfiguration.Q.fields.GyroDriftZ } + INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.GyroDriftX, + this->ekfConfiguration.Q.GyroDriftY, + this->ekfConfiguration.Q.GyroDriftZ } ); - INSSetBaroVar(this->ekfConfiguration.R.fields.BaroZ); + INSSetBaroVar(this->ekfConfiguration.R.BaroZ); // Initialize the gyro bias float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; @@ -294,7 +295,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros); - INSResetP(this->ekfConfiguration.P.data); + INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)); } else { // Run prediction a bit before any corrections @@ -368,21 +369,21 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (!this->usePos) { // position and velocity variance used in indoor mode - INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, - (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.FakeGPSPosIndoor }, + (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelIndoor, + this->ekfConfiguration.FakeR.FakeGPSVelIndoor, + this->ekfConfiguration.FakeR.FakeGPSVelIndoor } ); } else { // position and velocity variance used in outdoor mode - INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.fields.GPSPosNorth, - this->ekfConfiguration.R.fields.GPSPosEast, - this->ekfConfiguration.R.fields.GPSPosDown }, - (float[3]) { this->ekfConfiguration.R.fields.GPSVelNorth, - this->ekfConfiguration.R.fields.GPSVelEast, - this->ekfConfiguration.R.fields.GPSVelDown } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.GPSPosNorth, + this->ekfConfiguration.R.GPSPosEast, + this->ekfConfiguration.R.GPSPosDown }, + (float[3]) { this->ekfConfiguration.R.GPSVelNorth, + this->ekfConfiguration.R.GPSVelEast, + this->ekfConfiguration.R.GPSVelDown } ); } @@ -397,12 +398,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, - this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, - (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, - this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, - this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed } + INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.FakeGPSPosIndoor, + this->ekfConfiguration.FakeR.FakeGPSPosIndoor }, + (float[3]) { this->ekfConfiguration.FakeR.FakeGPSVelAirspeed, + this->ekfConfiguration.FakeR.FakeGPSVelAirspeed, + this->ekfConfiguration.FakeR.FakeGPSVelAirspeed } ); // rotate airspeed vector into NED frame - airspeed is measured in X axis only float R[3][3]; @@ -421,12 +422,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state) EKFStateVarianceData vardata; EKFStateVarianceGet(&vardata); - INSGetP(vardata.P.data); + INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); EKFStateVarianceSet(&vardata); int t; for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { - if (!IS_REAL(vardata.P.data[t]) || vardata.P.data[t] <= 0.0f) { - INSResetP(this->ekfConfiguration.P.data); + if (!IS_REAL(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t]) || cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t] <= 0.0f) { + INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)); this->init_stage = -1; break; } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index c3de29513..9967e7ebf 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -39,10 +39,11 @@ */ #include - +#include // private includes #include "inc/systemmod.h" + // UAVOs #include #include @@ -433,9 +434,9 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_ // By convention, there is a direct mapping between task monitor task_id's and members // of the TaskInfoXXXXElem enums PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); - taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; - taskData->StackRemaining.data[task_id] = task_info->stack_remaining; - taskData->RunningTime.data[task_id] = task_info->running_time_percentage; + cast_struct_to_array(taskData->Running, taskData->Running.System)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; + ((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining; + ((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage; } #endif diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index 6b180fd3a..f08fde249 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -51,7 +51,7 @@ */ #include "openpilot.h" - +#include #include "txpidsettings.h" #include "accessorydesired.h" #include "manualcontrolcommand.h" @@ -171,111 +171,116 @@ static void updatePIDs(UAVObjEvent *ev) // Loop through every enabled instance for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { - if (inst.PIDs.data[i] != TXPIDSETTINGS_PIDS_DISABLED) { + if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) { float value; - if (inst.Inputs.data[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { + if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { ManualControlCommandThrottleGet(&value); value = scale(value, - inst.ThrottleRange.fields.Min, - inst.ThrottleRange.fields.Max, - inst.MinPID.data[i], inst.MaxPID.data[i]); - } else if (AccessoryDesiredInstGet(inst.Inputs.data[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { - value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID.data[i], inst.MaxPID.data[i]); + inst.ThrottleRange.Min, + inst.ThrottleRange.Max, + cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i], + cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]); + } else if (AccessoryDesiredInstGet( + cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, + &accessory) == 0) { + value = scale(accessory.AccessoryVal, -1.0f, 1.0f, + cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i], + cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]); } else { continue; } - switch (inst.PIDs.data[i]) { + switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) { case TXPIDSETTINGS_PIDS_ROLLRATEKP: - needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); + needsUpdate |= update(&stab.RollRatePID.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKI: - needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); + needsUpdate |= update(&stab.RollRatePID.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKD: - needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); + needsUpdate |= update(&stab.RollRatePID.Kd, value); break; case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); + needsUpdate |= update(&stab.RollRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: - needsUpdate |= update(&stab.RollPI.fields.Kp, value); + needsUpdate |= update(&stab.RollPI.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: - needsUpdate |= update(&stab.RollPI.fields.Ki, value); + needsUpdate |= update(&stab.RollPI.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI.fields.ILimit, value); + needsUpdate |= update(&stab.RollPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKP: - needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); + needsUpdate |= update(&stab.PitchRatePID.Kp, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKI: - needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); + needsUpdate |= update(&stab.PitchRatePID.Ki, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKD: - needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); + needsUpdate |= update(&stab.PitchRatePID.Kd, value); break; case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: - needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); + needsUpdate |= update(&stab.PitchRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: - needsUpdate |= update(&stab.PitchPI.fields.Kp, value); + needsUpdate |= update(&stab.PitchPI.Kp, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: - needsUpdate |= update(&stab.PitchPI.fields.Ki, value); + needsUpdate |= update(&stab.PitchPI.Ki, value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); + needsUpdate |= update(&stab.PitchPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: - needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); - needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); + needsUpdate |= update(&stab.RollRatePID.Kp, value); + needsUpdate |= update(&stab.PitchRatePID.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: - needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); - needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); + needsUpdate |= update(&stab.RollRatePID.Ki, value); + needsUpdate |= update(&stab.PitchRatePID.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: - needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); - needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); + needsUpdate |= update(&stab.RollRatePID.Kd, value); + needsUpdate |= update(&stab.PitchRatePID.Kd, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: - needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); - needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); + needsUpdate |= update(&stab.RollRatePID.ILimit, value); + needsUpdate |= update(&stab.PitchRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: - needsUpdate |= update(&stab.RollPI.fields.Kp, value); - needsUpdate |= update(&stab.PitchPI.fields.Kp, value); + needsUpdate |= update(&stab.RollPI.Kp, value); + needsUpdate |= update(&stab.PitchPI.Kp, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: - needsUpdate |= update(&stab.RollPI.fields.Ki, value); - needsUpdate |= update(&stab.PitchPI.fields.Ki, value); + needsUpdate |= update(&stab.RollPI.Ki, value); + needsUpdate |= update(&stab.PitchPI.Ki, value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: - needsUpdate |= update(&stab.RollPI.fields.ILimit, value); - needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); + needsUpdate |= update(&stab.RollPI.ILimit, value); + needsUpdate |= update(&stab.PitchPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKP: - needsUpdate |= update(&stab.YawRatePID.fields.Kp, value); + needsUpdate |= update(&stab.YawRatePID.Kp, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKI: - needsUpdate |= update(&stab.YawRatePID.fields.Ki, value); + needsUpdate |= update(&stab.YawRatePID.Ki, value); break; case TXPIDSETTINGS_PIDS_YAWRATEKD: - needsUpdate |= update(&stab.YawRatePID.fields.Kd, value); + needsUpdate |= update(&stab.YawRatePID.Kd, value); break; case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: - needsUpdate |= update(&stab.YawRatePID.fields.ILimit, value); + needsUpdate |= update(&stab.YawRatePID.ILimit, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: - needsUpdate |= update(&stab.YawPI.fields.Kp, value); + needsUpdate |= update(&stab.YawPI.Kp, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: - needsUpdate |= update(&stab.YawPI.fields.Ki, value); + needsUpdate |= update(&stab.YawPI.Ki, value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: - needsUpdate |= update(&stab.YawPI.fields.ILimit, value); + needsUpdate |= update(&stab.YawPI.ILimit, value); break; case TXPIDSETTINGS_PIDS_GYROTAU: needsUpdate |= update(&stab.GyroTau, value); diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index cdc813bfa..0bc0a2f1c 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -47,7 +47,7 @@ */ #include - +#include #include "vtolpathfollower.h" #include "accelstate.h" @@ -337,8 +337,8 @@ static void updatePOIBearing() // don't try to move any closer if (poiRadius >= 3.0f || changeRadius > 0) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { - pathDesired.End.fields.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); - pathDesired.End.fields.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); + pathDesired.End.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.StartingVelocity = 1.0f; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -351,7 +351,7 @@ static void updatePOIBearing() /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ stabDesired.Yaw = yaw + (pathAngle / 2.0f); - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // cameraDesired.Yaw=yaw; // cameraDesired.PitchOrServo2=elevation; @@ -382,7 +382,10 @@ static void updatePathVelocity() { positionState.North, positionState.East, positionState.Down }; struct path_status progress; - path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); + 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) { @@ -414,7 +417,7 @@ static void updatePathVelocity() velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; - float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp; + float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp; float correction_velocity[2] = { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; @@ -427,13 +430,13 @@ static void updatePathVelocity() velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; - float altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * bound(progress.fractional_progress, 0, 1); + float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1); float downError = altitudeSetpoint - positionState.Down; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki, - -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit, - vtolpathfollowerSettings.VerticalPosPI.fields.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral); + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, + -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); // update pathstatus @@ -505,17 +508,17 @@ void updateEndpointVelocity() } // Compute desired north command - northError = pathDesired.End.fields.North - northPos; - northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + northPosIntegral); + northError = pathDesired.End.North - northPos; + northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral); - eastError = pathDesired.End.fields.East - eastPos; - eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki, - -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit, - vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + eastPosIntegral); + eastError = pathDesired.End.East - eastPos; + eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki, + -vtolpathfollowerSettings.HorizontalPosPI.ILimit, + vtolpathfollowerSettings.HorizontalPosPI.ILimit); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral); // Limit the maximum velocity float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); @@ -527,11 +530,11 @@ void updateEndpointVelocity() velocityDesired.North = northCommand * scale; velocityDesired.East = eastCommand * scale; - downError = pathDesired.End.fields.Down - downPos; - downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki, - -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit, - vtolpathfollowerSettings.VerticalPosPI.fields.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral); + downError = pathDesired.End.Down - downPos; + downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki, + -vtolpathfollowerSettings.VerticalPosPI.ILimit, + vtolpathfollowerSettings.VerticalPosPI.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); @@ -550,9 +553,9 @@ static void updateFixedAttitude(float *attitude) stabDesired.Pitch = attitude[1]; stabDesired.Yaw = attitude[2]; stabDesired.Throttle = attitude[3]; - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; StabilizationDesiredSet(&stabDesired); } @@ -629,31 +632,31 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) // Compute desired north command northError = velocityDesired.North - northVel; - northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit); - northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + northVelIntegral - - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd + northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.ILimit); + northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral + - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired east command eastError = velocityDesired.East - eastVel; - eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki, - -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit, - vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit); - eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + eastVelIntegral - - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd + eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki, + -vtolpathfollowerSettings.HorizontalVelPID.ILimit, + vtolpathfollowerSettings.HorizontalVelPID.ILimit); + eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral + - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); // Compute desired down command downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; - downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.fields.Ki, - -vtolpathfollowerSettings.VerticalVelPID.fields.ILimit, - vtolpathfollowerSettings.VerticalVelPID.fields.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.fields.Kp + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.fields.Kd); + downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, + -vtolpathfollowerSettings.VerticalVelPID.ILimit, + vtolpathfollowerSettings.VerticalVelPID.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); @@ -673,13 +676,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) stabDesired.Throttle = manualControl.Throttle; } - stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; if (yaw_attitude) { - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { - stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.fields.Yaw * manualControlData.Yaw; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; } StabilizationDesiredSet(&stabDesired); } diff --git a/flight/pios/inc/pios_struct_helper.h b/flight/pios/inc/pios_struct_helper.h index 1198b6fb1..65bae405a 100644 --- a/flight/pios/inc/pios_struct_helper.h +++ b/flight/pios/inc/pios_struct_helper.h @@ -17,4 +17,13 @@ (type *)((char *)__mptr - offsetof(type, member)); } \ ) +/** + * cast_struct_to_array casts an homogeneous structure instance to an array + * of typeof(struct_field). struct_field need to be any of the fields + * containing inside the struct + * @instance: homogeneous structure to cast + * @struct_field: a field contained inside the structure + */ +#define cast_struct_to_array(instance, struct_field) \ + ((typeof(struct_field) *) & (instance)) #endif /* PIOS_STRUCT_HELPER_H */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 672eac86e..2c2cea409 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -866,7 +866,7 @@ void PIOS_Board_Init(void) { HwSettingsData hwSettings; HwSettingsGet(&hwSettings); - if (hwSettings.OptionalModules.fields.Overo == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (hwSettings.OptionalModules.Overo == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 25eebe6ff..78e8855d6 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -126,19 +126,16 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) // Check if it a named set and creates structures accordingly if (info->fields[n]->numElements > 1) { if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { - QString unionTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); - QString unionType = QString("typedef union {\n"); - unionType.append(QString(" %1 data[%2];\n").arg(type).arg(info->fields[n]->numElements)); - unionType.append(QString(" struct __attribute__ ((__packed__)) {\n")); + QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); + QString structType = QString("typedef struct __attribute__ ((__packed__)) {\n"); for (int f = 0; f < info->fields[n]->elementNames.count(); f++) { - unionType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f])); + structType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f])); } - unionType.append(QString(" } fields;\n")); - unionType.append(QString("} %1 ;\n\n").arg(unionTypeName)); + structType.append(QString("} %1 ;\n\n").arg(structTypeName)); - dataStructures.append(unionType); + dataStructures.append(structType); - fields.append(QString(" %1 %2;\n").arg(unionTypeName) + fields.append(QString(" %1 %2;\n").arg(structTypeName) .arg(info->fields[n]->name)); } else { fields.append(QString(" %1 %2[%3];\n").arg(type) @@ -225,28 +222,25 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) } else { // Initialize all fields in the array for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { - QString optIdentifier; - if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { - optIdentifier = QString(".data"); + if (info->fields[n]->elementNames[0].compare(QString("0")) == 0) { + initfields.append(QString(" data.%1[%2] = ") + .arg(info->fields[n]->name) + .arg(idx)); + } else { + initfields.append(QString(" data.%1.%2 = ") + .arg(info->fields[n]->name) + .arg(info->fields[n]->elementNames[idx])); } + if (info->fields[n]->type == FIELDTYPE_ENUM) { - initfields.append(QString(" data.%1%2[%3] = %4;\n") - .arg(info->fields[n]->name) - .arg(optIdentifier) - .arg(idx) + initfields.append(QString("%1;\n") .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { - initfields.append(QString(" data.%1%2[%3] = %4f;\n") - .arg(info->fields[n]->name) - .arg(optIdentifier) - .arg(idx) + initfields.append(QString("%1f;\n") .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); } else { - initfields.append(QString(" data.%1%2[%3] = %4;\n") - .arg(info->fields[n]->name) - .arg(optIdentifier) - .arg(idx) + initfields.append(QString("%1;\n") .arg(info->fields[n]->defaultValues[idx].toInt())); } } From 0db9a9bf8d6cddc89261e02195cef8eded3e1111 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 1 Sep 2013 13:23:20 +0200 Subject: [PATCH 11/11] OP-1058 Add xxxGet and xxxSet functions to handle multielement fields as struct for example: EKFStateVariancePSet(EKFStateVariancePData *NewP); EKFStateVariancePGet(EKFStateVariancePData *NewP); Also in this case array accessors are renamed as xxxArrayGet/Set: EKFStateVariancePArraySet(float *NewP); EKFStateVariancePArrayGet(float *NewP); Nothing changes for anonymous items as default functions continues to deal with arrays +review OPReview-552 --- flight/libraries/sanitycheck.c | 6 +- flight/modules/Airspeed/airspeed.c | 2 +- flight/modules/Attitude/attitude.c | 2 +- flight/modules/Attitude/revolution/attitude.c | 2 +- flight/modules/Battery/battery.c | 2 +- flight/modules/CameraStab/camerastab.c | 6 +- flight/modules/ComUsbBridge/ComUsbBridge.c | 6 +- .../fixedwingpathfollower.c | 6 +- flight/modules/GPS/GPS.c | 6 +- flight/modules/Osd/osdoutput/osdoutput.c | 6 +- flight/modules/StateEstimation/filtercf.c | 2 +- flight/modules/TxPID/txpid.c | 6 +- .../VtolPathFollower/vtolpathfollower.c | 6 +- .../flight/uavobjectgeneratorflight.cpp | 75 +++++++++++++++++-- 14 files changed, 96 insertions(+), 37 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index c1bb65eb3..51442f48e 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -204,13 +204,13 @@ static int32_t check_stabilization_settings(int index, bool multirotor) // Get the different axis modes for this switch position switch (index) { case 1: - ManualControlSettingsStabilization1SettingsGet(modes); + ManualControlSettingsStabilization1SettingsArrayGet(modes); break; case 2: - ManualControlSettingsStabilization2SettingsGet(modes); + ManualControlSettingsStabilization2SettingsArrayGet(modes); break; case 3: - ManualControlSettingsStabilization3SettingsGet(modes); + ManualControlSettingsStabilization3SettingsArrayGet(modes); break; default: return SYSTEMALARMS_ALARM_ERROR; diff --git a/flight/modules/Airspeed/airspeed.c b/flight/modules/Airspeed/airspeed.c index 9241007d2..395cee43a 100644 --- a/flight/modules/Airspeed/airspeed.c +++ b/flight/modules/Airspeed/airspeed.c @@ -109,7 +109,7 @@ int32_t AirspeedInitialize() #endif uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); + HwSettingsADCRoutingArrayGet(adcRouting); // Determine if the barometric airspeed sensor is routed to an ADC pin for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index c64bc6272..84ee51bb3 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -647,7 +647,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples; // Z should average -grav attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; - attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; + attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; AttitudeSettingsSet(&attitudeSettings); } else { trim_requested = false; diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index ee616e44d..d502d1c8b 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -366,7 +366,7 @@ static int32_t updateAttitudeComplementary(bool first_run) magData.z = 0.0f; #endif float magBias[3]; - RevoCalibrationmag_biasGet(magBias); + 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; diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index b74094add..07c1a221c 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -91,7 +91,7 @@ int32_t BatteryInitialize(void) #endif uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; - HwSettingsADCRoutingGet(adcRouting); + HwSettingsADCRoutingArrayGet(adcRouting); // Determine if the battery sensors are routed to ADC pins for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) { diff --git a/flight/modules/CameraStab/camerastab.c b/flight/modules/CameraStab/camerastab.c index 6493022b9..b39a19492 100644 --- a/flight/modules/CameraStab/camerastab.c +++ b/flight/modules/CameraStab/camerastab.c @@ -97,12 +97,12 @@ int32_t CameraStabInitialize(void) #ifdef MODULE_CAMERASTAB_BUILTIN cameraStabEnabled = true; #else - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesData optionalModules; HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(&optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED) { cameraStabEnabled = true; } else { cameraStabEnabled = false; diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 11d0161be..7be0976ce 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -102,12 +102,12 @@ static int32_t comUsbBridgeInitialize(void) bridge_enabled = true; #else HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(&optionalModules); if (usart_port && vcp_port && - (optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) { + (optionalModules.ComUsbBridge == HWSETTINGS_OPTIONALMODULES_ENABLED)) { bridge_enabled = true; } else { bridge_enabled = false; diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index fe51f0eca..5858bf5e0 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -110,9 +110,9 @@ int32_t FixedWingPathFollowerStart() int32_t FixedWingPathFollowerInitialize() { HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + HwSettingsOptionalModulesData optionalModules; + HwSettingsOptionalModulesGet(&optionalModules); + if (optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) { followerEnabled = true; FixedWingPathFollowerSettingsInitialize(); FixedWingPathFollowerStatusInitialize(); diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 5d71e0daf..420f9599c 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -130,11 +130,11 @@ int32_t GPSInitialize(void) gpsEnabled = true; #else HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(&optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (optionalModules.GPS == HWSETTINGS_OPTIONALMODULES_ENABLED) { gpsEnabled = true; } else { gpsEnabled = false; diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 2157f3761..88f84d8f7 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -276,9 +276,9 @@ static int32_t osdoutputInitialize(void) osdoutputEnabled = 1; #else HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + HwSettingsOptionalModulesData optionalModules; + HwSettingsOptionalModulesGet(&optionalModules); + if (optionalModules.OsdHk == HWSETTINGS_OPTIONALMODULES_ENABLED) { osdoutputEnabled = 1; } else { osdoutputEnabled = 0; diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 224cf1953..50a30a3ed 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -229,7 +229,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel mag[2] = 0.0f; #endif float magBias[3]; - RevoCalibrationmag_biasGet(magBias); + 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) { this->magCalibrated = false; diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index f08fde249..125d921c0 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -88,12 +88,12 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM int32_t TxPIDInitialize(void) { bool txPIDEnabled; - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesData optionalModules; HwSettingsInitialize(); - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(&optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) { txPIDEnabled = true; } else { txPIDEnabled = false; diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 0bc0a2f1c..fad0b7b31 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -125,11 +125,11 @@ int32_t VtolPathFollowerStart() */ int32_t VtolPathFollowerInitialize() { - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesData optionalModules; - HwSettingsOptionalModulesGet(optionalModules); + HwSettingsOptionalModulesGet(&optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + if (optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) { VtolPathFollowerSettingsInitialize(); NedAccelInitialize(); PathDesiredInitialize(); diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 78e8855d6..72d4c1a63 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -280,11 +280,49 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) .arg(fieldTypeStrC[info->fields[n]->type])); setgetfields.append(QString("}\n")); } else { + // When no struct accessor is available for a field array accessor is the default. + QString suffix = QString(""); + + if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { + // struct based field accessor + QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); + /* SET */ + setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n") + .arg(structTypeName) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); + + /* GET */ + setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n") + .arg(structTypeName) + .arg(info->name) + .arg(info->fields[n]->name)); + setgetfields.append(QString("{\n")); + setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") + .arg(info->name) + .arg(info->fields[n]->name) + .arg(info->fields[n]->numElements) + .arg(fieldTypeStrC[info->fields[n]->type])); + setgetfields.append(QString("}\n")); + + // Append array suffix to array accessors + suffix = QString("Array"); + } + + // array based field accessor /* SET */ - setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n") + setgetfields.append(QString("void %2%3%4Set( %1 *New%3 )\n") .arg(fieldTypeStrC[info->fields[n]->type]) .arg(info->name) - .arg(info->fields[n]->name)); + .arg(info->fields[n]->name) + .arg(suffix)); setgetfields.append(QString("{\n")); setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") .arg(info->name) @@ -294,10 +332,11 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) setgetfields.append(QString("}\n")); /* GET */ - setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n") + setgetfields.append(QString("void %2%3%4Get( %1 *New%3 )\n") .arg(fieldTypeStrC[info->fields[n]->type]) .arg(info->name) - .arg(info->fields[n]->name)); + .arg(info->fields[n]->name) + .arg(suffix)); setgetfields.append(QString("{\n")); setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n") .arg(info->name) @@ -315,17 +354,37 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info) for (int n = 0; n < info->fields.length(); ++n) { // if (!info->fields[n]->defaultValues.isEmpty() ) { + QString suffix = QString(""); + if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { + // struct based field accessor + QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); + + /* SET */ + setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n") + .arg(structTypeName) + .arg(info->name) + .arg(info->fields[n]->name)); + + /* GET */ + setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n") + .arg(structTypeName) + .arg(info->name) + .arg(info->fields[n]->name)); + suffix = QString("Array"); + } /* SET */ - setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n") + setgetfieldsextern.append(QString("extern void %2%3%4Set(%1 *New%3);\n") .arg(fieldTypeStrC[info->fields[n]->type]) .arg(info->name) - .arg(info->fields[n]->name)); + .arg(info->fields[n]->name) + .arg(suffix)); /* GET */ - setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n") + setgetfieldsextern.append(QString("extern void %2%3%4Get(%1 *New%3);\n") .arg(fieldTypeStrC[info->fields[n]->type]) .arg(info->name) - .arg(info->fields[n]->name)); + .arg(info->fields[n]->name) + .arg(suffix)); } } outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern);