Commit 367b0b85 authored by Don Gagne's avatar Don Gagne

Add Guided mode support

parent cd39a194
This diff is collapsed.
......@@ -84,12 +84,15 @@ public:
QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
QList<MAV_CMD> supportedMissionCommands(void) final;
bool isCapable(FirmwareCapabilities capabilities) final;
bool isCapable(FirmwareCapabilities capabilities);
QStringList flightModes(void) final;
QString flightMode(uint8_t base_mode, uint32_t custom_mode) 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);
int manualControlReservedButtonCount(void) final;
void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
void adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
void adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) final;
void initializeVehicle(Vehicle* vehicle) final;
bool sendHomePositionToVehicle(void) final;
void addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
......@@ -111,6 +114,10 @@ private:
static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion);
void _setInfoSeverity(mavlink_message_t* message) const;
QString _getMessageText(mavlink_message_t* message) const;
void _handleParamValue(Vehicle* vehicle, mavlink_message_t* message);
void _handleParamSet(Vehicle* vehicle, mavlink_message_t* message);
void _handleStatusText(Vehicle* vehicle, mavlink_message_t* message);
void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
APMFirmwareVersion _firmwareVersion;
bool _textSeverityAdjustmentNeeded;
......
......@@ -25,6 +25,8 @@
/// @author Don Gagne <don@thegagnes.com>
#include "ArduCopterFirmwarePlugin.h"
#include "QGCApplication.h"
#include "MissionManager.h"
APMCopterMode::APMCopterMode(uint32_t mode, bool settable) :
APMCustomMode(mode, settable)
......@@ -73,3 +75,104 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void)
supportedFlightModes << APMCopterMode(APMCopterMode::BRAKE ,true);
setSupportedModes(supportedFlightModes);
}
bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities;
}
void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
vehicle->setFlightMode("RTL");
}
void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
vehicle->setFlightMode("Land");
}
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
return;
}
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF;
cmd.confirmation = 0;
cmd.param1 = 0.0f;
cmd.param2 = 0.0f;
cmd.param3 = 0.0f;
cmd.param4 = 0.0f;
cmd.param5 = 0.0f;
cmd.param6 = 0.0f;
cmd.param7 = vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel; // AMSL meters
cmd.target_system = vehicle->id();
cmd.target_component = 0;
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
vehicle->sendMessage(msg);
}
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;
}
vehicle->setFlightMode("Guided");
QGeoCoordinate coordWithAltitude = gotoCoord;
coordWithAltitude.setAltitude(vehicle->altitudeRelative()->rawValue().toDouble());
vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}
void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
return;
}
mavlink_message_t msg;
mavlink_set_position_target_local_ned_t cmd;
memset(&cmd, 0, sizeof(mavlink_set_position_target_local_ned_t));
cmd.target_system = vehicle->id();
cmd.target_component = 0;
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 = -(altitudeRel - vehicle->altitudeRelative()->rawValue().toDouble());
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_set_position_target_local_ned_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
vehicle->sendMessage(msg);
}
bool ArduCopterFirmwarePlugin::isPaused(const Vehicle* vehicle) const
{
return vehicle->flightMode() == "Brake";
}
void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
vehicle->setFlightMode("Brake");
}
void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
vehicle->setFlightMode("Guided");
} else {
pauseVehicle(vehicle);
}
}
......@@ -64,6 +64,17 @@ class ArduCopterFirmwarePlugin : public APMFirmwarePlugin
public:
ArduCopterFirmwarePlugin(void);
// Overrides from FirmwarePlugin
bool isCapable(FirmwareCapabilities capabilities) final;
bool isPaused(const Vehicle* vehicle) const 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, double altitudeRel) final;
void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
};
#endif
......@@ -22,9 +22,16 @@
======================================================================*/
#include "FirmwarePlugin.h"
#include "QGCApplication.h"
#include <QDebug>
bool FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
Q_UNUSED(capabilities);
return false;
}
QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
{
Q_UNUSED(vehicle);
......@@ -32,7 +39,7 @@ QList<VehicleComponent*> FirmwarePlugin::componentsForVehicle(AutoPilotPlugin* v
return QList<VehicleComponent*>();
}
QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
QString FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
{
QString flightMode;
......@@ -86,11 +93,17 @@ int FirmwarePlugin::manualControlReservedButtonCount(void)
return -1;
}
void FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
void FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
Q_UNUSED(vehicle);
Q_UNUSED(message);
// Generic plugin does no message adjustment
}
void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
Q_UNUSED(vehicle);
Q_UNUSED(message);
// Generic plugin does no message adjustment
}
......@@ -129,3 +142,69 @@ void FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataFile
majorVersion = -1;
minorVersion = -1;
}
bool FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
return false;
}
void FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
Q_UNUSED(vehicle);
Q_UNUSED(guidedMode);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
bool FirmwarePlugin::isPaused(const Vehicle* vehicle) const
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
return false;
}
void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
Q_UNUSED(altitudeRel);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
Q_UNUSED(gotoCoord);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
Q_UNUSED(altitudeRel);
qgcApp()->showMessage(QStringLiteral("Guided mode not supported by Vehicle."));
}
......@@ -50,12 +50,14 @@ class FirmwarePlugin : public QObject
public:
/// Set of optional capabilites which firmware may support
typedef enum {
SetFlightModeCapability, ///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location
GuidedModeCapability = 1 << 3, ///< Vehicle Support guided mode commands
} FirmwareCapabilities;
/// @return true: Firmware supports all specified capabilites
virtual bool isCapable(FirmwareCapabilities capabilities) { Q_UNUSED(capabilities); return false; }
virtual bool isCapable(FirmwareCapabilities capabilities);
/// Returns VehicleComponents for specified Vehicle
/// @param vehicle Vehicle to associate with components
......@@ -69,13 +71,42 @@ public:
/// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable.
/// @param base_mode Base mode from mavlink HEARTBEAT message
/// @param custom_mode Custom mode from mavlink HEARTBEAT message
virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode);
virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode) const;
/// Sets base_mode and custom_mode to specified flight mode.
/// @param[out] base_mode Base mode for SET_MODE mavlink message
/// @param[out] custom_mode Custom mode for SET_MODE mavlink message
virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
/// Returns whether the vehicle is in guided mode or not.
virtual bool isGuidedMode(const Vehicle* vehicle) const;
/// Set guided flight mode
virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode);
/// Returns whether the vehicle is paused or not.
virtual bool isPaused(const Vehicle* vehicle) const;
/// Causes the vehicle to stop at current position. If guide mode is supported, vehicle will be let in guide mode.
/// If not, vehicle will be left in Loiter.
virtual void pauseVehicle(Vehicle* vehicle);
/// Command vehicle to return to launch
virtual void guidedModeRTL(Vehicle* vehicle);
/// Command vehicle to land at current location
virtual void guidedModeLand(Vehicle* vehicle);
/// Command vehicle to takeoff from current location
/// @param altitudeRel Relative altitude to takeoff to
virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
/// Command vehicle to move to specified location (altitude is included and relative)
virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord);
/// Command vehicle to change to the specified relatice altitude
virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel);
/// FIXME: This isn't quite correct being here. All code for Joystick suvehicleTypepport is currently firmware specific
/// not just this. I'm going to try to change that. If not, this will need to be removed.
/// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
......@@ -89,8 +120,15 @@ public:
/// spec implementations such that the base code can remain mavlink generic.
/// @param vehicle Vehicle message came from
/// @param message[in,out] Mavlink message to adjust if needed.
virtual void adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
virtual void adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
/// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics.
/// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain
/// mavlink generic.
/// @param vehicle Vehicle message came from
/// @param message[in,out] Mavlink message to adjust if needed.
virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
/// Called when Vehicle is first created to send any necessary mavlink messages to the firmware.
virtual void initializeVehicle(Vehicle* vehicle);
......
......@@ -111,7 +111,7 @@ QStringList PX4FirmwarePlugin::flightModes(void)
return flightModes;
}
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode)
QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) const
{
QString flightMode = "Unknown";
......@@ -178,17 +178,10 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
return 0; // 0 buttons reserved for rc switch simulation
}
void PX4FirmwarePlugin::adjustMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message)
{
Q_UNUSED(vehicle);
Q_UNUSED(message);
// PX4 Flight Stack plugin does no message adjustment
}
bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
{
return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities;
qDebug() << (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability)) << capabilities;
return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
}
void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
......@@ -247,3 +240,8 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile)
metaData->loadParameterFactMetaDataFile(metaDataFile);
return metaData;
}
void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
vehicle->setFlightMode("Auto: Pause");
}
......@@ -43,10 +43,10 @@ public:
bool isCapable (FirmwareCapabilities capabilities) final;
QStringList flightModes (void) final;
QString flightMode (uint8_t base_mode, uint32_t custom_mode) 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;
void pauseVehicle (Vehicle* vehicle) final;
int manualControlReservedButtonCount(void) final;
void adjustMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final;
void initializeVehicle (Vehicle* vehicle) final;
bool sendHomePositionToVehicle (void) final;
void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final;
......
......@@ -47,22 +47,8 @@ QGCView {
QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
property real availableHeight: parent.height
readonly property bool isBackgroundDark: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true
property var _activeVehicle: multiVehicleManager.activeVehicle
readonly property real _defaultRoll: 0
readonly property real _defaultPitch: 0
readonly property real _defaultHeading: 0
readonly property real _defaultAltitudeAMSL: 0
readonly property real _defaultGroundSpeed: 0
readonly property real _defaultAirSpeed: 0
readonly property string _mapName: "FlightDisplayView"
readonly property string _showMapBackgroundKey: "/showMapBackground"
readonly property string _mainIsMapKey: "MainFlyWindowIsMap"
readonly property string _PIPVisibleKey: "IsPIPVisible"
property bool _mainIsMap: QGroundControl.loadBoolGlobalSetting(_mainIsMapKey, true)
property bool _isPipVisible: QGroundControl.loadBoolGlobalSetting(_PIPVisibleKey, true)
......@@ -82,6 +68,18 @@ QGCView {
property real pipSize: mainWindow.width * 0.2
readonly property bool isBackgroundDark: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true
readonly property real _defaultRoll: 0
readonly property real _defaultPitch: 0
readonly property real _defaultHeading: 0
readonly property real _defaultAltitudeAMSL: 0
readonly property real _defaultGroundSpeed: 0
readonly property real _defaultAirSpeed: 0
readonly property string _mapName: "FlightDisplayView"
readonly property string _showMapBackgroundKey: "/showMapBackground"
readonly property string _mainIsMapKey: "MainFlyWindowIsMap"
readonly property string _PIPVisibleKey: "IsPIPVisible"
FlightDisplayViewController { id: _controller }
function setStates() {
......@@ -174,6 +172,7 @@ QGCView {
FlightDisplayViewMap {
id: _flightMap
anchors.fill: parent
flightWidgets: widgetsLoader.item
}
}
......
......@@ -40,9 +40,14 @@ FlightMap {
anchors.fill: parent
mapName: _mapName
property alias missionController: _missionController
property var flightWidgets
property bool _followVehicle: true
property bool _activeVehicleCoordinateValid: multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.coordinateValid : false
property var activeVehicleCoordinate: multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.coordinate : QtPositioning.coordinate()
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _activeVehicleCoordinateValid: _activeVehicle ? _activeVehicle.coordinateValid : false
property var activeVehicleCoordinate: _activeVehicle ? _activeVehicle.coordinate : QtPositioning.coordinate()
property var _gotoHereCoordinate: QtPositioning.coordinate()
Component.onCompleted: {
QGroundControl.flightMapPosition = center
......@@ -58,6 +63,8 @@ FlightMap {
}
}
QGCPalette { id: qgcPal; colorGroupEnabled: true }
MissionController {
id: _missionController
Component.onCompleted: start(false /* editMode */)
......@@ -68,14 +75,14 @@ FlightMap {
model: _mainIsMap ? multiVehicleManager.activeVehicle ? multiVehicleManager.activeVehicle.trajectoryPoints : 0 : 0
delegate:
MapPolyline {
line.width: 3
line.color: "red"
z: QGroundControl.zOrderMapItems - 1
path: [
{ latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
{ latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
]
}
line.width: 3
line.color: "red"
z: QGroundControl.zOrderMapItems - 1
path: [
{ latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
{ latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
]
}
}
// Add the vehicles to the map
......@@ -83,12 +90,12 @@ FlightMap {
model: multiVehicleManager.vehicles
delegate:
VehicleMapItem {
vehicle: object
coordinate: object.coordinate
isSatellite: flightMap.isSatelliteMap
size: _mainIsMap ? ScreenTools.defaultFontPixelHeight * 5 : ScreenTools.defaultFontPixelHeight * 2
z: QGroundControl.zOrderMapItems
}
vehicle: object
coordinate: object.coordinate
isSatellite: flightMap.isSatelliteMap
size: _mainIsMap ? ScreenTools.defaultFontPixelHeight * 5 : ScreenTools.defaultFontPixelHeight * 2
z: QGroundControl.zOrderMapItems
}
}
// Add the mission items to the map
......@@ -101,8 +108,29 @@ FlightMap {
model: _mainIsMap ? _missionController.waypointLines : 0
}
// Used to make pinch zoom work
// GoTo here waypoint
MapQuickItem {
coordinate: _gotoHereCoordinate
visible: _vehicle.guidedMode && _gotoHereCoordinate.isValid
z: QGroundControl.zOrderMapItems
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
sourceItem: MissionItemIndexLabel {
isCurrentItem: true
label: "G"
}
}
// Handle guided mode clicks
MouseArea {
anchors.fill: parent
onClicked: {
if (_activeVehicle && _activeVehicle.guidedMode) {
_gotoHereCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y))
flightWidgets.guidedModeBar.confirmAction(flightWidgets.guidedModeBar.confirmGoTo)
}
}
}
}
......@@ -107,6 +107,41 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& missionItems)
emit inProgressChanged(true);
}
void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly)
{
if (inProgress()) {
qCDebug(MissionManagerLog) << "writeArduPilotGuidedMissionItem called while transaction in progress";
return;
}
_writeTransactionInProgress = true;
mavlink_message_t messageOut;
mavlink_mission_item_t missionItem;
missionItem.target_system = _vehicle->id();
missionItem.target_component = 0;
missionItem.seq = 0;
missionItem.command = MAV_CMD_NAV_WAYPOINT;
missionItem.param1 = 0;
missionItem.param2 = 0;
missionItem.param3 = 0;
missionItem.param4 = 0;
missionItem.x = gotoCoord.latitude();
missionItem.y = gotoCoord.longitude();
missionItem.z = gotoCoord.altitude();
missionItem.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
missionItem.current = altChangeOnly ? 3 : 2;
missionItem.autocontinue = true;
mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem);
_dedicatedLink = _vehicle->priorityLink();
_vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
_startAckTimeout(AckGuidedItem);
emit inProgressChanged(true);
}
void MissionManager::requestMissionItems(void)
{
qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
......@@ -211,8 +246,6 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
}
_requestNextMissionItem();
}
}
void MissionManager::_requestNextMissionItem(void)
......@@ -394,6 +427,16 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
_finishTransaction(false);
}
break;
case AckGuidedItem:
// MISSION_REQUEST is expected, or MISSION_ACK to end sequence
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck guide mode item accepted";
_finishTransaction(true);
} else {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle did not accept guided item.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
_finishTransaction(false);
}
break;
}
}
......@@ -437,14 +480,16 @@ void MissionManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
QString MissionManager::_ackTypeToString(AckType_t ackType)
{
switch (ackType) {
case AckNone: // State machine is idle
case AckNone:
return QString("No Ack");
case AckMissionCount: // MISSION_COUNT message expected
case AckMissionCount:
return QString("MISSION_COUNT");
case AckMissionItem: ///< MISSION_ITEM expected
case AckMissionItem:
return QString("MISSION_ITEM");
case AckMissionRequest: ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
case AckMissionRequest:
return QString("MISSION_REQUEST");
case AckGuidedItem:
return QString("Guided Mode Item");
default:
qWarning(MissionManagerLog) << "Fell off end of switch statement";
return QString("QGC Internal Error");
......
......@@ -58,6 +58,11 @@ public:
/// @param missionItems Items to send to vehicle
void writeMissionItems(const QList<MissionItem*>& missionItems);
/// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
/// @param gotoCoord Coordinate to move to
/// @param altChangeOnly true: only altitude change, false: lat/lon/alt change
void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);
/// Error codes returned in error signal
typedef enum {
InternalError,
......@@ -90,6 +95,7 @@ private:
AckMissionCount, ///< MISSION_COUNT message expected
AckMissionItem, ///< MISSION_ITEM expected
AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
AckGuidedItem, ///< MISSION_ACK expected in reponse to ArduPilot guided mode single item send
} AckType_t;
void _startAckTimeout(AckType_t ack);
......
......@@ -99,6 +99,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _rcRSSI(0)
, _rcRSSIstore(100.0)
, _autoDisconnect(false)
, _flying(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _missionManager(NULL)
......@@ -140,7 +141,7 @@ Vehicle::Vehicle(LinkInterface* link,
connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection);
connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_announceflightModeChanged);
connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged);
connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged);
_uas = new UAS(_mavlink, this, _firmwarePluginManager);
......@@ -376,7 +377,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
}
// Give the plugin a change to adjust the message contents
_firmwarePlugin->adjustMavlinkMessage(this, &message);
_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message);
switch (message.msgid) {
case MAVLINK_MSG_ID_HOME_POSITION:
......@@ -412,6 +413,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_VIBRATION:
_handleVibration(message);
break;
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
_handleExtendedSysState(message);
break;
// Following are ArduPilot dialect messages
......@@ -425,6 +429,23 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas->receiveMessage(message);
}
void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
{
mavlink_extended_sys_state_t extendedState;
mavlink_msg_extended_sys_state_decode(&message, &