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

Merge pull request #5636 from DonLakeFlyer/APMGuided 

Better Guided Action support for ArduPilot
QGC4.4
Don Gagne 8 years ago committed by GitHub
parent
432971acd2 724df478bf
commit
c28aba664d
12 changed files with 100 additions and 103 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. 85
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 10
      src/FirmwarePlugin/APM/APMFirmwarePlugin.h
  3. 72
      src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
  4. 8
      src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
  5. 7
      src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h
  6. 8
      src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
  7. 4
      src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
  8. 2
      src/FlightDisplay/FlightDisplayViewMap.qml
  9. 2
      src/FlightDisplay/GuidedActionsController.qml
  10. 2
      src/MissionManager/MissionManager.cc
  11. 1
      src/MissionManager/MissionSettingsItem.cc
  12. 2
      src/Vehicle/Vehicle.cc

85
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
Unescape View File

@ -7,10 +7,6 @@ @@ -7,10 +7,6 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "APMFirmwarePlugin.h"
#include "APMAutoPilotPlugin.h"
#include "QGCMAVLink.h"
@ -18,6 +14,7 @@ @@ -18,6 +14,7 @@
#include "APMFlightModesComponentController.h"
#include "APMAirframeComponentController.h"
#include "APMSensorsComponentController.h"
#include "MissionManager.h"
#include <QTcpSocket>
@ -159,9 +156,13 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle) @@ -159,9 +156,13 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
return new APMAutoPilotPlugin(vehicle, vehicle);
}
bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
{
return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
Q_UNUSED(vehicle);
uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
return (capabilities & vehicleCapabilities) == capabilities;
}
QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
@ -725,12 +726,6 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const @@ -725,12 +726,6 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
return vehicle->flightMode() == "Guided";
}
void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
// Best we can do in this case
vehicle->setFlightMode("Loiter");
}
void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle)
{
Q_UNUSED(vehicle);
@ -819,3 +814,69 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle) @@ -819,3 +814,69 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
return QString();
}
}
void APMFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
_setFlightModeAndValidate(vehicle, "Guided");
} else {
pauseVehicle(vehicle);
}
}
void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
_setFlightModeAndValidate(vehicle, pauseFlightMode());
}
void APMFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
return;
}
setGuidedMode(vehicle, true);
QGeoCoordinate coordWithAltitude = gotoCoord;
coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}
void APMFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
_setFlightModeAndValidate(vehicle, rtlFlightMode());
}
void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
return;
}
setGuidedMode(vehicle, true);
mavlink_message_t msg;
mavlink_set_position_target_local_ned_t cmd;
memset(&cmd, 0, sizeof(cmd));
cmd.target_system = vehicle->id();
cmd.target_component = vehicle->defaultComponentId();
cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
cmd.type_mask = 0xFFF8; // Only x/y/z valid
cmd.x = 0.0f;
cmd.y = 0.0f;
cmd.z = -(altitudeChange);
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
vehicle->priorityLink()->mavlinkChannel(),
&msg,
&cmd);
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}

10
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
Unescape View File

@ -74,12 +74,18 @@ public: @@ -74,12 +74,18 @@ public:
QList<MAV_CMD> supportedMissionCommands(void) final;
AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) final;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) final;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
QStringList flightModes (Vehicle* vehicle) final;
QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final;
bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
bool isGuidedMode (const Vehicle* vehicle) const final;
void pauseVehicle (Vehicle* vehicle) override;
QString rtlFlightMode (void) const final { return QString("RTL"); }
QString missionFlightMode (void) const final { return QString("Auto"); }
void pauseVehicle (Vehicle* vehicle) final;
void guidedModeRTL (Vehicle* vehicle) final;
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) override;
int manualControlReservedButtonCount(void) override;
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) override;
void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) final;

72
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
Unescape View File

@ -151,20 +151,6 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe @@ -151,20 +151,6 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
return majorVersionNumber == 3 ? 5 : Vehicle::versionNotSetValue;
}
bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
{
Q_UNUSED(vehicle);
uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
return (capabilities & vehicleCapabilities) == capabilities;
}
void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
_setFlightModeAndValidate(vehicle, "RTL");
}
void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
_setFlightModeAndValidate(vehicle, "Land");
@ -209,64 +195,6 @@ bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle) @@ -209,64 +195,6 @@ bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
return true;
}
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known."));
return;
}
QGeoCoordinate coordWithAltitude = gotoCoord;
coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}
void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
return;
}
setGuidedMode(vehicle, true);
mavlink_message_t msg;
mavlink_set_position_target_local_ned_t cmd;
memset(&cmd, 0, sizeof(cmd));
cmd.target_system = vehicle->id();
cmd.target_component = vehicle->defaultComponentId();
cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED;
cmd.type_mask = 0xFFF8; // Only x/y/z valid
cmd.x = 0.0f;
cmd.y = 0.0f;
cmd.z = -(altitudeChange);
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
vehicle->priorityLink()->mavlinkChannel(),
&msg,
&cmd);
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}
void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
_setFlightModeAndValidate(vehicle, "Brake");
}
void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
_setFlightModeAndValidate(vehicle, "Guided");
} else {
pauseVehicle(vehicle);
}
}
bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle)
{
Q_UNUSED(vehicle);

8
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
Unescape View File

@ -55,22 +55,14 @@ public: @@ -55,22 +55,14 @@ public:
ArduCopterFirmwarePlugin(void);
// Overrides from FirmwarePlugin
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) final;
void pauseVehicle (Vehicle* vehicle) final;
void guidedModeRTL (Vehicle* vehicle) final;
void guidedModeLand (Vehicle* vehicle) final;
void guidedModeTakeoff (Vehicle* vehicle) final;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
bool multiRotorXConfig (Vehicle* vehicle) final;
QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Brake"); }
QString missionFlightMode (void) const override { return QString("Auto"); }
QString rtlFlightMode (void) const override { return QString("RTL"); }
QString landFlightMode (void) const override { return QString("Land"); }
QString takeControlFlightMode (void) const override { return QString("Loiter"); }
bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const final;

7
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h
Unescape View File

@ -56,10 +56,11 @@ public: @@ -56,10 +56,11 @@ public:
ArduPlaneFirmwarePlugin(void);
// Overrides from FirmwarePlugin
QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
QString pauseFlightMode (void) const override { return QString("Loiter"); }
QString offlineEditingParamFile (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
QString autoDisarmParameter (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); }
int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
QString autoDisarmParameter(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("LAND_DISARMDELAY"); }
private:
static bool _remapParamNameIntialized;

8
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc
Unescape View File

@ -8,6 +8,7 @@ @@ -8,6 +8,7 @@
****************************************************************************/
#include "ArduRoverFirmwarePlugin.h"
#include "QGCApplication.h"
bool ArduRoverFirmwarePlugin::_remapParamNameIntialized = false;
FirmwarePlugin::remapParamNameMajorVersionMap_t ArduRoverFirmwarePlugin::_remapParamName;
@ -98,3 +99,10 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer @@ -98,3 +99,10 @@ int ArduRoverFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer
return majorVersionNumber == 3 ? 2 : Vehicle::versionNotSetValue;
}
void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
Q_UNUSED(vehicle);
Q_UNUSED(altitudeChange);
qgcApp()->showMessage(QStringLiteral("Change altitude not supported."));
}

4
src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h
Unescape View File

@ -51,8 +51,10 @@ public: @@ -51,8 +51,10 @@ public:
ArduRoverFirmwarePlugin(void);
// Overrides from FirmwarePlugin
QString pauseFlightMode (void) const override { return QString("Hold"); }
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange) final;
int remapParamNameHigestMinorVersionNumber (int majorVersionNumber) const final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
private:
static bool _remapParamNameIntialized;

2
src/FlightDisplay/FlightDisplayViewMap.qml
Unescape View File

@ -261,7 +261,7 @@ FlightMap { @@ -261,7 +261,7 @@ FlightMap {
// GoTo here waypoint
MapQuickItem {
coordinate: _gotoHereCoordinate
visible: _activeVehicle && _activeVehicle.guidedMode && _gotoHereCoordinate.isValid
visible: _activeVehicle && _activeVehicle.guidedModeSupported && _gotoHereCoordinate.isValid
z: QGroundControl.zOrderMapItems
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY

2
src/FlightDisplay/GuidedActionsController.qml
Unescape View File

@ -101,7 +101,7 @@ Item { @@ -101,7 +101,7 @@ Item {
property bool showChangeAlt: (_activeVehicle && _vehicleFlying) && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showOrbit: !_hideOrbit && _activeVehicle && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive
property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding
property bool showGotoLocation: _activeVehicle && _activeVehicle.guidedMode && _vehicleFlying
property bool showGotoLocation: _activeVehicle && _vehicleFlying
property bool guidedUIVisible: guidedActionConfirm.visible || guidedActionList.visible

2
src/MissionManager/MissionManager.cc
Unescape View File

@ -40,6 +40,8 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC @@ -40,6 +40,8 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC
_transactionInProgress = TransactionWrite;
_connectToMavlink();
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;

1
src/MissionManager/MissionSettingsItem.cc
Unescape View File

@ -233,7 +233,6 @@ void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate) @@ -233,7 +233,6 @@ void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate)
if (_plannedHomePositionCoordinate != coordinate) {
// ArduPilot tends to send crap home positions at initial vehicel boot, discard them
if (coordinate.isValid() && (coordinate.latitude() != 0 || coordinate.longitude() != 0)) {
qDebug() << "Setting home position" << coordinate;
_plannedHomePositionCoordinate = coordinate;
emit coordinateChanged(coordinate);
emit exitCoordinateChanged(coordinate);

2
src/Vehicle/Vehicle.cc
Unescape View File

@ -1733,8 +1733,6 @@ void Vehicle::setFlightMode(const QString& flightMode) @@ -1733,8 +1733,6 @@ void Vehicle::setFlightMode(const QString& flightMode)
uint8_t base_mode;
uint32_t custom_mode;
qDebug() << flightMode;
if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
// setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing
// states.

Write Preview
Loading…
Cancel
Save