zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Call AutoPilotPlugin instead of hard-wired code

QGC4.4
Don Gagne 10 years ago
parent
dbb90dd092
commit
e56080f8b9
4 changed files with 22 additions and 157 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      src/comm/px4_custom_mode.h
  2. 53
      src/uas/UAS.cc
  3. 68
      src/ui/uas/UASControlWidget.cc
  4. 16
      src/ui/uas/UASControlWidget.h

42
src/comm/px4_custom_mode.h
Unescape View File

@ -1,42 +0,0 @@ @@ -1,42 +0,0 @@
/*
* 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_ */

53
src/uas/UAS.cc
Unescape View File

@ -28,7 +28,7 @@ @@ -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) @@ -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)
{

68
src/ui/uas/UASControlWidget.cc
Unescape View File

@ -40,24 +40,10 @@ This file is part of the PIXHAWK project @@ -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), @@ -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() @@ -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()) {
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()));

16
src/ui/uas/UASControlWidget.h
Unescape View File

@ -38,12 +38,7 @@ This file is part of the QGROUNDCONTROL project @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save