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

Merge pull request #3574 from dagar/px4_modes 

PX4 mode cleanup
QGC4.4
Don Gagne 9 years ago committed by GitHub
parent
3caf40072b 3808a1f9b5
commit
033f52bd85
6 changed files with 116 additions and 93 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. 1
      qgroundcontrol.pro
  2. 6
      src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml
  3. 90
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  4. 2
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
  5. 77
      src/FirmwarePlugin/PX4/px4_custom_mode.h
  6. 33
      src/comm/MockLink.cc

1
qgroundcontrol.pro
Unescape View File

@ -664,6 +664,7 @@ HEADERS+= \ @@ -664,6 +664,7 @@ HEADERS+= \
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h \
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h \
src/FirmwarePlugin/PX4/px4_custom_mode.h \
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \
src/FirmwarePlugin/PX4/PX4ParameterMetaData.h \
src/Vehicle/MultiVehicleManager.h \

6
src/AutoPilotPlugins/PX4/PX4AdvancedFlightModes.qml
Unescape View File

@ -63,7 +63,7 @@ Item { @@ -63,7 +63,7 @@ Item {
readonly property string fwAcroModeDescription: qsTr("The angular rates are controlled, but not the attitude. ")
readonly property string mrAcroModeDescription: qsTr("The angular rates are controlled, but not the attitude. ")
readonly property string altCtlModeName: qsTr("Altitude Control")
readonly property string altCtlModeName: qsTr("Altitude")
readonly property string fwAltCtlModeDescription: qsTr("Roll stick controls banking, pitch stick altitude ") +
qsTr("Throttle stick controls speed. ") +
qsTr("With no stick inputs the plane holds heading, but drifts off in wind. ")
@ -76,10 +76,10 @@ Item { @@ -76,10 +76,10 @@ Item {
readonly property string mrPosCtlModeDescription: qsTr("Roll and Pitch sticks control sideways and forward speed ") +
qsTr("Throttle stick controls climb / sink rade. ")
readonly property string missionModeName: qsTr("Auto Mission")
readonly property string missionModeName: qsTr("Mission")
readonly property string missionModeDescription: qsTr("The aircraft obeys the programmed mission sent by QGroundControl. ")
readonly property string loiterModeName: qsTr("Auto Pause")
readonly property string loiterModeName: qsTr("Hold")
readonly property string fwLoiterModeDescription: qsTr("The aircraft flies in a circle around the current position at the current altitude. ")
readonly property string mrLoiterModeDescription: qsTr("The multirotor hovers at the current position and altitude. ")

90
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
Unescape View File

@ -18,37 +18,7 @@ @@ -18,37 +18,7 @@
#include <QDebug>
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,
PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE
};
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,
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME
};
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
#include "px4_custom_mode.h"
struct Modes2Name {
uint8_t main_mode;
@ -60,39 +30,43 @@ struct Modes2Name { @@ -60,39 +30,43 @@ struct Modes2Name {
};
const char* PX4FirmwarePlugin::manualFlightMode = "Manual";
const char* PX4FirmwarePlugin::altCtlFlightMode = "Altitude";
const char* PX4FirmwarePlugin::posCtlFlightMode = "Position";
const char* PX4FirmwarePlugin::missionFlightMode = "Mission";
const char* PX4FirmwarePlugin::holdFlightMode = "Hold";
const char* PX4FirmwarePlugin::takeoffFlightMode = "Takeoff";
const char* PX4FirmwarePlugin::landingFlightMode = "Land";
const char* PX4FirmwarePlugin::rtlFlightMode = "Return";
const char* PX4FirmwarePlugin::acroFlightMode = "Acro";
const char* PX4FirmwarePlugin::offboardFlightMode = "Offboard";
const char* PX4FirmwarePlugin::stabilizedFlightMode = "Stabilized";
const char* PX4FirmwarePlugin::rattitudeFlightMode = "Rattitude";
const char* PX4FirmwarePlugin::altCtlFlightMode = "Altitude Control";
const char* PX4FirmwarePlugin::posCtlFlightMode = "Position Control";
const char* PX4FirmwarePlugin::offboardFlightMode = "Offboard Control";
const char* PX4FirmwarePlugin::readyFlightMode = "Ready";
const char* PX4FirmwarePlugin::takeoffFlightMode = "Takeoff";
const char* PX4FirmwarePlugin::pauseFlightMode = "Hold";
const char* PX4FirmwarePlugin::missionFlightMode = "Mission";
const char* PX4FirmwarePlugin::rtlFlightMode = "Return To Land";
const char* PX4FirmwarePlugin::landingFlightMode = "Landing";
const char* PX4FirmwarePlugin::rtgsFlightMode = "Return, Link Loss";
const char* PX4FirmwarePlugin::followMeFlightMode = "Follow Me";
const char* PX4FirmwarePlugin::rtgsFlightMode = "Return to Groundstation";
const char* PX4FirmwarePlugin::readyFlightMode = "Ready"; // unused
/// Tranlates from PX4 custom modes to flight mode names
static const struct Modes2Name rgModes2Name[] = {
{ PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::manualFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::acroFlightMode, true, false, true},
{ PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::stabilizedFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::rattitudeFlightMode, true, false, true},
{ PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::altCtlFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::posCtlFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::offboardFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::readyFlightMode, false, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::pauseFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::missionFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::rtlFlightMode, true, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::landingFlightMode, false, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::rtgsFlightMode, false, true, true},
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME, PX4FirmwarePlugin::followMeFlightMode, true, true, true},
//main_mode sub_mode name canBeSet FW MC
{ PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::manualFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::stabilizedFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::acroFlightMode, true, false, true },
{ PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::rattitudeFlightMode, true, false, true },
{ PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::altCtlFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::posCtlFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::holdFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::missionFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::rtlFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4FirmwarePlugin::followMeFlightMode, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::offboardFlightMode, true, true, true },
// modes that can't be directly set by the user
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::landingFlightMode, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::readyFlightMode, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::rtgsFlightMode, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::takeoffFlightMode, false, true, true },
};
QList<VehicleComponent*> PX4FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
@ -379,7 +353,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu @@ -379,7 +353,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
vehicle->setFlightMode(pauseFlightMode);
vehicle->setFlightMode(holdFlightMode);
} else {
pauseVehicle(vehicle);
}
@ -388,6 +362,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) @@ -388,6 +362,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
// Not supported by generic vehicle
return (vehicle->flightMode() == pauseFlightMode || vehicle->flightMode() == takeoffFlightMode
return (vehicle->flightMode() == holdFlightMode || vehicle->flightMode() == takeoffFlightMode
|| vehicle->flightMode() == landingFlightMode);
}

2
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
Unescape View File

@ -63,7 +63,7 @@ public: @@ -63,7 +63,7 @@ public:
static const char* offboardFlightMode;
static const char* readyFlightMode;
static const char* takeoffFlightMode;
static const char* pauseFlightMode;
static const char* holdFlightMode;
static const char* missionFlightMode;
static const char* rtlFlightMode;
static const char* landingFlightMode;

77
src/FirmwarePlugin/PX4/px4_custom_mode.h
Unescape View File

@ -0,0 +1,77 @@ @@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4_custom_mode.h
* PX4 custom flight modes
*
*/
#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,
PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE
};
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,
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET
};
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_ */

33
src/comm/MockLink.cc
Unescape View File

@ -18,6 +18,8 @@ @@ -18,6 +18,8 @@
#include <string.h>
#include "px4_custom_mode.h"
QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog")
QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
@ -26,37 +28,6 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") @@ -26,37 +28,6 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
///
/// @author Don Gagne <don@thegagnes.com>
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,
PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE
};
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;
};
float MockLink::_vehicleLatitude = 47.633033f;
float MockLink::_vehicleLongitude = -122.08794f;
float MockLink::_vehicleAltitude = 3.5f;

Write Preview
Loading…
Cancel
Save