Skip to content

Q
qgroundcontrol
Commit c374d0e1 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #5652 from DonLakeFlyer/GuidedTakeoff 

Guided takeoff
parents 47a9895a 13cd3784
Hide whitespace changes
Inline Side-by-side
Showing with 147 additions and 118 deletions
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @ c374d0e1
......@@ -15,6 +15,7 @@
#include "APMAirframeComponentController.h"
#include "APMSensorsComponentController.h"
#include "MissionManager.h"
#include "ParameterManager.h"
#include <QTcpSocket>
......@@ -158,11 +159,18 @@ AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
bool APMFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabilities capabilities)
{
Q_UNUSED(vehicle);
uint32_t vehicleCapabilities = SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability;
uint32_t available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
if (vehicle->multiRotor()) {
available |= TakeoffVehicleCapability;
} else if (vehicle->fixedWing()) {
// Quad plane supports takeoff
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) &&
vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) {
available |= TakeoffVehicleCapability;
}
}
return (capabilities & vehicleCapabilities) == capabilities;
return (capabilities & available) == capabilities;
}
QList<VehicleComponent*> APMFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle)
......@@ -880,3 +888,88 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}
bool APMFirmwarePlugin::isVtol(const Vehicle* vehicle) const
{
if (vehicle->fixedWing()) {
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE")) &&
vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("Q_ENABLE"))->rawValue().toBool()) {
return true;
}
}
return false;
}
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
_guidedModeTakeoff(vehicle);
}
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
{
QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters
float takeoffAlt = 0;
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
takeoffAlt = takeoffAltFact->rawValue().toDouble();
}
if (takeoffAlt <= 0) {
takeoffAlt = 2.5;
} else {
takeoffAlt /= paramDivisor; // centimeters -> meters
}
if (!_setFlightModeAndValidate(vehicle, "Guided")) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
return false;
}
// FIXME: Is this needed?
if (!_armVehicleAndValidate(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return false;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
takeoffAlt);
return true;
}
// FIXME: Review for a better way to do this
void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
if (!vehicle->flying()) {
if (_guidedModeTakeoff(vehicle)) {
// Wait for vehicle to get off ground before switching to auto (10 seconds)
bool didTakeoff = false;
for (int i=0; i<100; i++) {
if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
didTakeoff = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
if (!didTakeoff) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
return;
}
}
}
if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
return;
}
}
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ c374d0e1
......@@ -74,9 +74,12 @@ public:
QList<MAV_CMD> supportedMissionCommands(void) final;
AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) final;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
bool isVtol (const Vehicle* vehicle) const final;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) final;
void guidedModeTakeoff (Vehicle* vehicle) final;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void startMission (Vehicle* vehicle) 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;
......@@ -120,7 +123,8 @@ private:
bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message);
void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
void _soloVideoHandshake(Vehicle* vehicle);
void _soloVideoHandshake(Vehicle* vehicle);
bool _guidedModeTakeoff(Vehicle* vehicle);
// Any instance data here must be global to all vehicles
// Vehicle specific data should go into APMFirmwarePluginInstanceData
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ c374d0e1
......@@ -160,45 +160,6 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
_setFlightModeAndValidate(vehicle, "Land");
}
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
_guidedModeTakeoff(vehicle);
}
bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
{
QString takeoffAltParam("PILOT_TKOFF_ALT");
float takeoffAlt = 0;
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
takeoffAlt = takeoffAltFact->rawValue().toDouble();
}
if (takeoffAlt <= 0) {
takeoffAlt = 2.5;
} else {
takeoffAlt /= 100; // centimeters -> meters
}
if (!_setFlightModeAndValidate(vehicle, "Guided")) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
return false;
}
if (!_armVehicleAndValidate(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return false;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
takeoffAlt);
return true;
}
bool ArduCopterFirmwarePlugin::multiRotorCoaxialMotors(Vehicle* vehicle)
{
Q_UNUSED(vehicle);
......@@ -223,34 +184,3 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle*
return true;
}
void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle)
{
double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
if (!vehicle->flying()) {
if (_guidedModeTakeoff(vehicle)) {
// Wait for vehicle to get off ground before switching to auto (10 seconds)
bool didTakeoff = false;
for (int i=0; i<100; i++) {
if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
didTakeoff = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
if (!didTakeoff) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
return;
}
}
}
if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
return;
}
}
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h
View file @ c374d0e1
......@@ -57,7 +57,6 @@ public:
// Overrides from FirmwarePlugin
void guidedModeLand (Vehicle* vehicle) final;
void guidedModeTakeoff (Vehicle* vehicle) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors (Vehicle* vehicle) final;
......@@ -68,13 +67,10 @@ public:
QString takeControlFlightMode (void) const override { return QString("Loiter"); }
bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const final;
QString autoDisarmParameter (Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); }
void startMission (Vehicle* vehicle) override;
private:
static bool _remapParamNameIntialized;
static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName;
bool _guidedModeTakeoff(Vehicle* vehicle);
};
#endif
src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.h
View file @ c374d0e1
......@@ -60,7 +60,6 @@ public:
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; }
private:
static bool _remapParamNameIntialized;
......
src/FirmwarePlugin/FirmwarePlugin.cc
View file @ c374d0e1
......@@ -530,3 +530,19 @@ bool FirmwarePlugin::hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitc
yawSupported = false;
return false;
}
bool FirmwarePlugin::isVtol(const Vehicle* vehicle) const
{
switch (vehicle->vehicleType()) {
case MAV_TYPE_VTOL_DUOROTOR:
case MAV_TYPE_VTOL_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
case MAV_TYPE_VTOL_RESERVED2:
case MAV_TYPE_VTOL_RESERVED3:
case MAV_TYPE_VTOL_RESERVED4:
case MAV_TYPE_VTOL_RESERVED5:
return true;
default:
return false;
}
}
src/FirmwarePlugin/FirmwarePlugin.h
View file @ c374d0e1
......@@ -45,6 +45,7 @@ public:
PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location
GuidedModeCapability = 1 << 3, ///< Vehicle supports guided mode commands
OrbitModeCapability = 1 << 4, ///< Vehicle supports orbit mode
TakeoffVehicleCapability = 1 << 5, ///< Vehicle supports guided takeoff
} FirmwareCapabilities;
/// Maps from on parameter name to another
......@@ -286,6 +287,9 @@ public:
/// @return true: vehicle has gimbal, false: gimbal support unknown
virtual bool hasGimbal(Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported);
/// Returns true if the vehicle is a VTOL
virtual bool isVtol(const Vehicle* vehicle) const;
// FIXME: Hack workaround for non pluginize FollowMe support
static const char* px4FollowMeFlightMode;
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file @ c374d0e1
......@@ -227,11 +227,12 @@ bool PX4FirmwarePlugin::supportsManualControl(void)
bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
{
if (vehicle->multiRotor()) {
return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability /*| OrbitModeCapability still NYI*/)) == capabilities;
} else {
return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
int available = MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
if (vehicle->multiRotor() || vehicle->vtol()) {
available |= TakeoffVehicleCapability;
}
return (capabilities & available) == capabilities;
}
void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
......
src/FlightDisplay/GuidedActionsController.qml
View file @ c374d0e1
......@@ -92,7 +92,7 @@ Item {
property bool showArm: _activeVehicle && !_vehicleArmed
property bool showDisarm: _activeVehicle && _vehicleArmed && !_vehicleFlying
property bool showRTL: _activeVehicle && _vehicleArmed && _activeVehicle.guidedModeSupported && _vehicleFlying && !_vehicleInRTLMode
property bool showTakeoff: _activeVehicle && _activeVehicle.guidedModeSupported && !_vehicleFlying && !_activeVehicle.fixedWing
property bool showTakeoff: _activeVehicle && _activeVehicle.takeoffVehicleSupported && !_vehicleFlying
property bool showLand: _activeVehicle && _activeVehicle.guidedModeSupported && _vehicleArmed && !_activeVehicle.fixedWing && !_vehicleInLandMode
property bool showStartMission: _activeVehicle && _missionAvailable && !_missionActive && !_vehicleFlying
property bool showContinueMission: _activeVehicle && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1)
......
src/Vehicle/Vehicle.cc
View file @ c374d0e1
......@@ -2094,18 +2094,7 @@ bool Vehicle::multiRotor(void) const
bool Vehicle::vtol(void) const
{
switch (vehicleType()) {
case MAV_TYPE_VTOL_DUOROTOR:
case MAV_TYPE_VTOL_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
case MAV_TYPE_VTOL_RESERVED2:
case MAV_TYPE_VTOL_RESERVED3:
case MAV_TYPE_VTOL_RESERVED4:
case MAV_TYPE_VTOL_RESERVED5:
return true;
default:
return false;
}
return _firmwarePlugin->isVtol(this);
}
bool Vehicle::supportsManualControl(void) const
......@@ -2224,6 +2213,11 @@ bool Vehicle::orbitModeSupported() const
return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
}
bool Vehicle::takeoffVehicleSupported() const
{
return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability);
}
void Vehicle::guidedModeRTL(void)
{
if (!guidedModeSupported()) {
......
src/Vehicle/Vehicle.h
View file @ c374d0e1
......@@ -319,23 +319,14 @@ public:
Q_PROPERTY(QmlObjectListModel* adsbVehicles READ adsbVehicles CONSTANT)
Q_PROPERTY(bool initialPlanRequestComplete READ initialPlanRequestComplete NOTIFY initialPlanRequestCompleteChanged)
/// true: Vehicle is flying, false: Vehicle is on ground
Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged)
/// true: Vehicle is flying, false: Vehicle is on ground
Q_PROPERTY(bool landing READ landing NOTIFY landingChanged)
/// true: Vehicle is in Guided mode and can respond to guided commands, false: vehicle cannot respond to direct control commands
Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged)
/// true: Guided mode commands are supported by this vehicle
Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT)
/// true: pauseVehicle command is supported
Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT)
/// true: Orbit mode is supported by this vehicle
Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT)
// Vehicle state used for guided control
Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying
Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) ///< Vehicle is in landing pattern (DO_LAND_START)
Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) ///< Vehicle is in Guided mode and can respond to guided commands
Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle
Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported
Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle
Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported
Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT)
......@@ -441,9 +432,10 @@ public:
Q_INVOKABLE void motorTest(int motor, int percent, int timeoutSecs);
#endif
bool guidedModeSupported(void) const;
bool pauseVehicleSupported(void) const;
bool orbitModeSupported(void) const;
bool guidedModeSupported (void) const;
bool pauseVehicleSupported (void) const;
bool orbitModeSupported (void) const;
bool takeoffVehicleSupported(void) const;
// Property accessors
......
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