Skip to content

Q
qgroundcontrol
Commit c9a43c9c authored by Don Gagne's avatar Don Gagne
Browse files
Options

Call AutoPilotPlugin instead of hard-wired code

parent 7982eb0f
Hide whitespace changes
Inline Side-by-side
Showing with 22 additions and 157 deletions
src/comm/px4_custom_mode.h deleted 100644 → 0
View file @ 7982eb0f
/*
* px4_custom_mode.h
*
* Created on: 09.08.2013
* Author: ton
*/
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
#include <stdint.h>
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
#endif /* PX4_CUSTOM_MODE_H_ */
src/uas/UAS.cc
View file @ c9a43c9c
......@@ -28,7 +28,7 @@
#include "SerialLink.h"
#include "UASParameterCommsMgr.h"
#include <Eigen/Geometry>
#include <comm/px4_custom_mode.h>
#include "AutoPilotPlugin.h"
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
......@@ -3326,56 +3326,7 @@ QString UAS::getAudioModeTextFor(int id)
*/
QString UAS::getShortModeTextFor(uint8_t base_mode, uint32_t custom_mode, int autopilot)
{
QString mode = "";
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
// use custom_mode - autopilot-specific
if (autopilot == MAV_AUTOPILOT_PX4) {
union px4_custom_mode px4_mode;
px4_mode.data = custom_mode;
if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
mode += "|MANUAL";
} else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
mode += "|ALTCTL";
} else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
mode += "|POSCTL";
} else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
mode += "|AUTO";
if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_READY) {
mode += "|READY";
} else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF) {
mode += "|TAKEOFF";
} else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LOITER) {
mode += "|LOITER";
} else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_MISSION) {
mode += "|MISSION";
} else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) {
mode += "|RTL";
} else if (px4_mode.sub_mode == PX4_CUSTOM_SUB_MODE_AUTO_LAND) {
mode += "|LAND";
}
} else if (px4_mode.main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
mode += "|OFFBOARD";
}
}
}
// fallback to using base_mode
if (mode.length() == 0) {
// use base_mode - not autopilot-specific
if (base_mode == 0) {
mode += "|PREFLIGHT";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_AUTO) {
mode += "|AUTO";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) {
mode += "|MANUAL";
if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_GUIDED) {
mode += "|GUIDED";
} else if (base_mode & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE) {
mode += "|STABILIZED";
}
}
}
QString mode = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot)->getShortModeText(base_mode, custom_mode);
if (mode.length() == 0)
{
......
src/ui/uas/UASControlWidget.cc
View file @ c9a43c9c
......@@ -40,24 +40,10 @@ This file is part of the PIXHAWK project
#include <UASManager.h>
#include <UAS.h>
#include "QGC.h"
static struct full_mode_s modes_list_common[] = {
{ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED,
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED),
0 },
{ (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
{ (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED),
0 },
};
static struct full_mode_s modes_list_px4[5];
#include "AutoPilotPlugin.h"
UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
uasID(-1),
modesList(NULL),
modesNum(0),
modeIdx(0),
armed(false)
{
......@@ -74,47 +60,23 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
void UASControlWidget::updateModesList()
{
union px4_custom_mode px4_cm;
px4_cm.data = 0;
modes_list_px4[0].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
modes_list_px4[0].customMode = px4_cm.data;
modes_list_px4[1].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
modes_list_px4[1].customMode = px4_cm.data;
modes_list_px4[2].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
modes_list_px4[2].customMode = px4_cm.data;
modes_list_px4[3].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
modes_list_px4[3].customMode = px4_cm.data;
modes_list_px4[4].baseMode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
modes_list_px4[4].customMode = px4_cm.data;
// Detect autopilot type
int autopilot = 0;
int autopilot = MAV_AUTOPILOT_GENERIC;
if (this->uasID >= 0) {
UASInterface *uas = UASManager::instance()->getUASForId(this->uasID);
if (uas) {
autopilot = UASManager::instance()->getUASForId(this->uasID)->getAutopilotType();
}
}
// Use corresponding modes list
if (autopilot == MAV_AUTOPILOT_PX4) {
modesList = modes_list_px4;
modesNum = sizeof(modes_list_px4) / sizeof(struct full_mode_s);
} else {
modesList = modes_list_common;
modesNum = sizeof(modes_list_common) / sizeof(struct full_mode_s);
}
AutoPilotPlugin* autopilotPlugin = AutoPilotPlugin::getInstanceForAutoPilotPlugin(autopilot);
_modeList = autopilotPlugin->getModes();
// Set combobox items
ui.modeComboBox->clear();
for (int i = 0; i < modesNum; i++) {
struct full_mode_s mode = modesList[i];
ui.modeComboBox->insertItem(i, UAS::getShortModeTextFor(mode.baseMode, mode.customMode, autopilot).remove(0, 2), i);
foreach (AutoPilotPlugin::FullMode_t fullMode, _modeList) {
ui.modeComboBox->addItem(UAS::getShortModeTextFor(fullMode.baseMode, fullMode.customMode, autopilot).remove(0, 2));
}
// Select first mode in list
......@@ -235,24 +197,24 @@ void UASControlWidget::transmitMode()
{
UASInterface* uas_iface = UASManager::instance()->getUASForId(this->uasID);
if (uas_iface) {
if (modeIdx >= 0 && modeIdx < modesNum) {
struct full_mode_s mode = modesList[modeIdx];
if (modeIdx >= 0 && modeIdx < _modeList.count()) {
AutoPilotPlugin::FullMode_t fullMode = _modeList[modeIdx];
// include armed state
if (armed) {
mode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
fullMode.baseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
mode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
fullMode.baseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
UAS* uas = dynamic_cast<UAS*>(uas_iface);
if (uas->isHilEnabled() || uas->isHilActive()) {
mode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED;
fullMode.baseMode |= MAV_MODE_FLAG_HIL_ENABLED;
} else {
mode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED;
fullMode.baseMode &= ~MAV_MODE_FLAG_HIL_ENABLED;
}
uas->setMode(mode.baseMode, mode.customMode);
uas->setMode(fullMode.baseMode, fullMode.customMode);
QString modeText = ui.modeComboBox->currentText();
ui.lastActionLabel->setText(QString("Sent new mode %1 to %2").arg(modeText).arg(uas->getUASName()));
......
src/ui/uas/UASControlWidget.h
View file @ c9a43c9c
......@@ -38,12 +38,7 @@ This file is part of the QGROUNDCONTROL project
#include <QPushButton>
#include <ui_UASControl.h>
#include <UASInterface.h>
#include <comm/px4_custom_mode.h>
struct full_mode_s {
uint8_t baseMode;
uint32_t customMode;
};
#include "AutoPilotPlugin.h"
/**
* @brief Widget controlling one MAV
......@@ -83,11 +78,10 @@ protected slots:
void setBackgroundColor(QColor color);
protected:
int uasID; ///< Reference to the current uas
struct full_mode_s *modesList; ///< Modes list for the current UAS
int modesNum; ///< Number of modes in list for the current UAS
int modeIdx; ///< Current uas mode index
bool armed; ///< Engine state
int uasID; ///< Reference to the current uas
QList<AutoPilotPlugin::FullMode_t> _modeList; ///< Mode list for the current UAS
int modeIdx; ///< Current uas mode index
bool armed; ///< Engine state
private:
Ui::uasControl ui;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment