diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f6d37cd..d7a411c 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -51,6 +51,7 @@ #include "VehicleBatteryFactGroup.h" #include "EventHandler.h" #include "Actuators/Actuators.h" +#include "GimbalController.h" #ifdef QT_DEBUG #include "MockLink.h" #endif @@ -501,6 +502,8 @@ void Vehicle::_commonInit() // enable Joystick if appropriate _loadJoystickSettings(); + + _gimbalController = new GimbalController(_mavlink, this); } Vehicle::~Vehicle() @@ -515,6 +518,9 @@ Vehicle::~Vehicle() delete _mav; _mav = nullptr; + + delete _gimbalController; + _gimbalController = nullptr; } void Vehicle::prepareDelete() @@ -758,9 +764,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_PING: _handlePing(link, message); break; - case MAVLINK_MSG_ID_MOUNT_ORIENTATION: - _handleGimbalOrientation(message); - break; case MAVLINK_MSG_ID_OBSTACLE_DISTANCE: _handleObstacleDistance(message); break; @@ -2628,9 +2631,10 @@ QString Vehicle::vehicleTypeName() const { { MAV_TYPE_VTOL_TAILSITTER_DUOROTOR, tr("Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter")}, { MAV_TYPE_VTOL_TAILSITTER_QUADROTOR, tr("Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter")}, { MAV_TYPE_VTOL_TILTROTOR, tr("Tiltrotor VTOL")}, - { MAV_TYPE_VTOL_FIXEDROTOR, tr("VTOL Fixedrotor")}, - { MAV_TYPE_VTOL_TAILSITTER, tr("VTOL Tailsitter")}, + { MAV_TYPE_VTOL_FIXEDROTOR, tr("VTOL Fixedrotor")}, + { MAV_TYPE_VTOL_TAILSITTER, tr("VTOL Tailsitter")}, { MAV_TYPE_VTOL_TILTWING, tr("VTOL Tiltwing")}, + { MAV_TYPE_VTOL_TILTWING, tr("VTOL reserved 4")}, { MAV_TYPE_VTOL_RESERVED5, tr("VTOL reserved 5")}, { MAV_TYPE_GIMBAL, tr("Onboard gimbal")}, { MAV_TYPE_ADSB, tr("Onboard ADSB peripheral")}, @@ -2849,6 +2853,15 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord) { + if (!centerCoord.isValid()) { + return; + } + // Sanity check Ardupilot. Max altitude processed is 83000 + if (apmFirmware()) { + if ((centerCoord.altitude() >= 83000) || (centerCoord.altitude() <= -83000 )) { + return; + } + } if (!roiModeSupported()) { qgcApp()->showAppMessage(QStringLiteral("ROI mode not supported by Vehicle.")); return; @@ -2857,7 +2870,7 @@ void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord) sendMavCommandInt( defaultComponentId(), MAV_CMD_DO_SET_ROI_LOCATION, - MAV_FRAME_GLOBAL, + apmFirmware() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL, true, // show error if fails static_cast(qQNaN()), // Empty static_cast(qQNaN()), // Empty @@ -3062,7 +3075,9 @@ void Vehicle::sendMavCommandIntWithHandler(const MavCmdAckHandlerInfo_t* ackHand bool Vehicle::isMavCommandPending(int targetCompId, MAV_CMD command) { - return ((-1) < _findMavCommandListEntryIndex(targetCompId, command)); + bool pending = ((-1) < _findMavCommandListEntryIndex(targetCompId, command)); + // qDebug() << "Pending target: " << targetCompId << ", command: " << (int)command << ", pending: " << (pending ? "yes" : "no"); + return pending; } int Vehicle::_findMavCommandListEntryIndex(int targetCompId, MAV_CMD command) @@ -4290,69 +4305,6 @@ void Vehicle::_altitudeAboveTerrainReceived(bool success, QList heights) _altitudeAboveTerrTerrainAtCoordinateQuery = nullptr; } -void Vehicle::gimbalControlValue(double pitch, double yaw) -{ - //qDebug() << "Gimbal:" << pitch << yaw; - sendMavCommand( - _defaultComponentId, - MAV_CMD_DO_MOUNT_CONTROL, - false, // show errors - static_cast(pitch), // Pitch 0 - 90 - 0, // Roll (not used) - static_cast(yaw), // Yaw -180 - 180 - 0, // Altitude (not used) - 0, // Latitude (not used) - 0, // Longitude (not used) - MAV_MOUNT_MODE_MAVLINK_TARGETING); // MAVLink Roll,Pitch,Yaw -} - -void Vehicle::gimbalPitchStep(int direction) -{ - if(_haveGimbalData) { - //qDebug() << "Pitch:" << _curGimbalPitch << direction << (_curGimbalPitch + direction); - double p = static_cast(_curGimbalPitch + direction); - gimbalControlValue(p, static_cast(_curGimbalYaw)); - } -} - -void Vehicle::gimbalYawStep(int direction) -{ - if(_haveGimbalData) { - //qDebug() << "Yaw:" << _curGimbalYaw << direction << (_curGimbalYaw + direction); - double y = static_cast(_curGimbalYaw + direction); - gimbalControlValue(static_cast(_curGimbalPitch), y); - } -} - -void Vehicle::centerGimbal() -{ - if(_haveGimbalData) { - gimbalControlValue(0.0, 0.0); - } -} - -void Vehicle::_handleGimbalOrientation(const mavlink_message_t& message) -{ - mavlink_mount_orientation_t o; - mavlink_msg_mount_orientation_decode(&message, &o); - if(fabsf(_curGimbalRoll - o.roll) > 0.5f) { - _curGimbalRoll = o.roll; - emit gimbalRollChanged(); - } - if(fabsf(_curGimbalPitch - o.pitch) > 0.5f) { - _curGimbalPitch = o.pitch; - emit gimbalPitchChanged(); - } - if(fabsf(_curGimbalYaw - o.yaw) > 0.5f) { - _curGimbalYaw = o.yaw; - emit gimbalYawChanged(); - } - if(!_haveGimbalData) { - _haveGimbalData = true; - emit gimbalDataChanged(); - } -} - void Vehicle::_handleObstacleDistance(const mavlink_message_t& message) { mavlink_obstacle_distance_t o; @@ -4396,6 +4348,7 @@ void Vehicle::_handleFenceStatus(const mavlink_message_t& message) lastUpdate = now; } } + void Vehicle::updateFlightDistance(double distance) { _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + distance); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index edc765a..1034e0f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -78,6 +78,7 @@ class LinkManager; class InitialConnectStateMachine; class Autotune; class RemoteIDManager; +class GimbalController; namespace events { namespace parser { @@ -97,7 +98,7 @@ class Vehicle : public FactGroup friend class SendMavCommandWithSignallingTest; // Unit test friend class SendMavCommandWithHandlerTest; // Unit test friend class RequestMessageTest; // Unit test - + friend class GimbalController; // Allow GimbalController to call _addFactGroup public: Vehicle(LinkInterface* link, @@ -243,10 +244,7 @@ public: Q_PROPERTY(quint64 mavlinkReceivedCount READ mavlinkReceivedCount NOTIFY mavlinkStatusChanged) Q_PROPERTY(quint64 mavlinkLossCount READ mavlinkLossCount NOTIFY mavlinkStatusChanged) Q_PROPERTY(float mavlinkLossPercent READ mavlinkLossPercent NOTIFY mavlinkStatusChanged) - Q_PROPERTY(qreal gimbalRoll READ gimbalRoll NOTIFY gimbalRollChanged) - Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) - Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) - Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) + Q_PROPERTY(GimbalController* gimbalController READ gimbalController CONSTANT) Q_PROPERTY(bool hasGripper READ hasGripper CONSTANT) Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged) @@ -449,11 +447,7 @@ public: Q_ENUM(PIDTuningTelemetryMode) Q_INVOKABLE void setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode); - - Q_INVOKABLE void gimbalControlValue (double pitch, double yaw); - Q_INVOKABLE void gimbalPitchStep (int direction); - Q_INVOKABLE void gimbalYawStep (int direction); - Q_INVOKABLE void centerGimbal (); + Q_INVOKABLE void forceArm (); /// Sends PARAM_MAP_RC message to vehicle @@ -909,12 +903,10 @@ public: quint64 mavlinkLossCount () const{ return _mavlinkLossCount; } /// Total number of lost messages float mavlinkLossPercent () const{ return _mavlinkLossPercent; } /// Running loss rate - qreal gimbalRoll () const{ return static_cast(_curGimbalRoll);} - qreal gimbalPitch () const{ return static_cast(_curGimbalPitch); } - qreal gimbalYaw () const{ return static_cast(_curGimbalYaw); } - bool gimbalData () const{ return _haveGimbalData; } bool isROIEnabled () const{ return _isROIEnabled; } + GimbalController* gimbalController () { return _gimbalController; } + CheckList checkListState () { return _checkListState; } void setCheckListState (CheckList cl) { _checkListState = cl; emit checkListStateChanged(); } @@ -1028,10 +1020,6 @@ signals: void requestProtocolVersion (unsigned version); void mavlinkStatusChanged (); - void gimbalRollChanged (); - void gimbalPitchChanged (); - void gimbalYawChanged (); - void gimbalDataChanged (); void isROIEnabledChanged (); void initialConnectComplete (); @@ -1217,6 +1205,7 @@ private: ComponentInformationManager* _componentInformationManager = nullptr; VehicleObjectAvoidance* _objectAvoidance = nullptr; Autotune* _autotune = nullptr; + GimbalController* _gimbalController = nullptr; bool _armed = false; ///< true: vehicle is armed uint8_t _base_mode = 0; ///< base_mode from HEARTBEAT @@ -1257,10 +1246,6 @@ private: uint8_t _compID = 0; bool _heardFrom = false; - float _curGimbalRoll = 0.0f; - float _curGimbalPitch = 0.0f; - float _curGimbalYaw = 0.0f; - bool _haveGimbalData = false; bool _isROIEnabled = false; Joystick* _activeJoystick = nullptr;