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

Vehicle: add gimbalController, and move gimbal functionality there: 

All the gimbal related commands, minus ROI, are now in gimbal controller.
This commit removes everything else related to gimbal in vehicle, and also
adds some small fixes

Co-authored-by: davidsastresas <davidsastresas@gmail.com>
Co-authored-by: Julian Oes <julian@oes.ch>
QGC4.4
davidsastresas 1 year ago committed by Julian Oes
parent
13ed857037
commit
c0d70cbc82
No known key found for this signature in database
GPG Key ID: F0ED380FEA56DE41
2 changed files with 30 additions and 92 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 93
      src/Vehicle/Vehicle.cc
  2. 29
      src/Vehicle/Vehicle.h

93
src/Vehicle/Vehicle.cc
Unescape View File

@ -51,6 +51,7 @@
#include "VehicleBatteryFactGroup.h" #include "VehicleBatteryFactGroup.h"
#include "EventHandler.h" #include "EventHandler.h"
#include "Actuators/Actuators.h" #include "Actuators/Actuators.h"
#include "GimbalController.h"
#ifdef QT_DEBUG #ifdef QT_DEBUG
#include "MockLink.h" #include "MockLink.h"
#endif #endif
@ -501,6 +502,8 @@ void Vehicle::_commonInit()
// enable Joystick if appropriate // enable Joystick if appropriate
_loadJoystickSettings(); _loadJoystickSettings();
_gimbalController = new GimbalController(_mavlink, this);
} }
Vehicle::~Vehicle() Vehicle::~Vehicle()
@ -515,6 +518,9 @@ Vehicle::~Vehicle()
delete _mav; delete _mav;
_mav = nullptr; _mav = nullptr;
delete _gimbalController;
_gimbalController = nullptr;
} }
void Vehicle::prepareDelete() void Vehicle::prepareDelete()
@ -758,9 +764,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_PING: case MAVLINK_MSG_ID_PING:
_handlePing(link, message); _handlePing(link, message);
break; break;
case MAVLINK_MSG_ID_MOUNT_ORIENTATION:
_handleGimbalOrientation(message);
break;
case MAVLINK_MSG_ID_OBSTACLE_DISTANCE: case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
_handleObstacleDistance(message); _handleObstacleDistance(message);
break; 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_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_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_TILTROTOR, tr("Tiltrotor VTOL")},
{ MAV_TYPE_VTOL_FIXEDROTOR, tr("VTOL Fixedrotor")}, { MAV_TYPE_VTOL_FIXEDROTOR, tr("VTOL Fixedrotor")},
{ MAV_TYPE_VTOL_TAILSITTER, tr("VTOL Tailsitter")}, { MAV_TYPE_VTOL_TAILSITTER, tr("VTOL Tailsitter")},
{ MAV_TYPE_VTOL_TILTWING, tr("VTOL Tiltwing")}, { 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_VTOL_RESERVED5, tr("VTOL reserved 5")},
{ MAV_TYPE_GIMBAL, tr("Onboard gimbal")}, { MAV_TYPE_GIMBAL, tr("Onboard gimbal")},
{ MAV_TYPE_ADSB, tr("Onboard ADSB peripheral")}, { 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) 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()) { if (!roiModeSupported()) {
qgcApp()->showAppMessage(QStringLiteral("ROI mode not supported by Vehicle.")); qgcApp()->showAppMessage(QStringLiteral("ROI mode not supported by Vehicle."));
return; return;
@ -2857,7 +2870,7 @@ void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord)
sendMavCommandInt( sendMavCommandInt(
defaultComponentId(), defaultComponentId(),
MAV_CMD_DO_SET_ROI_LOCATION, MAV_CMD_DO_SET_ROI_LOCATION,
MAV_FRAME_GLOBAL, apmFirmware() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL,
true, // show error if fails true, // show error if fails
static_cast<float>(qQNaN()), // Empty static_cast<float>(qQNaN()), // Empty
static_cast<float>(qQNaN()), // Empty static_cast<float>(qQNaN()), // Empty
@ -3062,7 +3075,9 @@ void Vehicle::sendMavCommandIntWithHandler(const MavCmdAckHandlerInfo_t* ackHand
bool Vehicle::isMavCommandPending(int targetCompId, MAV_CMD command) 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) int Vehicle::_findMavCommandListEntryIndex(int targetCompId, MAV_CMD command)
@ -4290,69 +4305,6 @@ void Vehicle::_altitudeAboveTerrainReceived(bool success, QList<double> heights)
_altitudeAboveTerrTerrainAtCoordinateQuery = nullptr; _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<float>(pitch), // Pitch 0 - 90
0, // Roll (not used)
static_cast<float>(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<double>(_curGimbalPitch + direction);
gimbalControlValue(p, static_cast<double>(_curGimbalYaw));
}
}
void Vehicle::gimbalYawStep(int direction)
{
if(_haveGimbalData) {
//qDebug() << "Yaw:" << _curGimbalYaw << direction << (_curGimbalYaw + direction);
double y = static_cast<double>(_curGimbalYaw + direction);
gimbalControlValue(static_cast<double>(_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) void Vehicle::_handleObstacleDistance(const mavlink_message_t& message)
{ {
mavlink_obstacle_distance_t o; mavlink_obstacle_distance_t o;
@ -4396,6 +4348,7 @@ void Vehicle::_handleFenceStatus(const mavlink_message_t& message)
lastUpdate = now; lastUpdate = now;
} }
} }
void Vehicle::updateFlightDistance(double distance) void Vehicle::updateFlightDistance(double distance)
{ {
_flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + distance); _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + distance);

29
src/Vehicle/Vehicle.h
Unescape View File

@ -78,6 +78,7 @@ class LinkManager;
class InitialConnectStateMachine; class InitialConnectStateMachine;
class Autotune; class Autotune;
class RemoteIDManager; class RemoteIDManager;
class GimbalController;
namespace events { namespace events {
namespace parser { namespace parser {
@ -97,7 +98,7 @@ class Vehicle : public FactGroup
friend class SendMavCommandWithSignallingTest; // Unit test friend class SendMavCommandWithSignallingTest; // Unit test
friend class SendMavCommandWithHandlerTest; // Unit test friend class SendMavCommandWithHandlerTest; // Unit test
friend class RequestMessageTest; // Unit test friend class RequestMessageTest; // Unit test
friend class GimbalController; // Allow GimbalController to call _addFactGroup
public: public:
Vehicle(LinkInterface* link, Vehicle(LinkInterface* link,
@ -243,10 +244,7 @@ public:
Q_PROPERTY(quint64 mavlinkReceivedCount READ mavlinkReceivedCount NOTIFY mavlinkStatusChanged) Q_PROPERTY(quint64 mavlinkReceivedCount READ mavlinkReceivedCount NOTIFY mavlinkStatusChanged)
Q_PROPERTY(quint64 mavlinkLossCount READ mavlinkLossCount NOTIFY mavlinkStatusChanged) Q_PROPERTY(quint64 mavlinkLossCount READ mavlinkLossCount NOTIFY mavlinkStatusChanged)
Q_PROPERTY(float mavlinkLossPercent READ mavlinkLossPercent NOTIFY mavlinkStatusChanged) Q_PROPERTY(float mavlinkLossPercent READ mavlinkLossPercent NOTIFY mavlinkStatusChanged)
Q_PROPERTY(qreal gimbalRoll READ gimbalRoll NOTIFY gimbalRollChanged) Q_PROPERTY(GimbalController* gimbalController READ gimbalController CONSTANT)
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(bool hasGripper READ hasGripper CONSTANT) Q_PROPERTY(bool hasGripper READ hasGripper CONSTANT)
Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged)
Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged) Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged)
@ -449,11 +447,7 @@ public:
Q_ENUM(PIDTuningTelemetryMode) Q_ENUM(PIDTuningTelemetryMode)
Q_INVOKABLE void setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode); 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 (); Q_INVOKABLE void forceArm ();
/// Sends PARAM_MAP_RC message to vehicle /// Sends PARAM_MAP_RC message to vehicle
@ -909,12 +903,10 @@ public:
quint64 mavlinkLossCount () const{ return _mavlinkLossCount; } /// Total number of lost messages quint64 mavlinkLossCount () const{ return _mavlinkLossCount; } /// Total number of lost messages
float mavlinkLossPercent () const{ return _mavlinkLossPercent; } /// Running loss rate float mavlinkLossPercent () const{ return _mavlinkLossPercent; } /// Running loss rate
qreal gimbalRoll () const{ return static_cast<qreal>(_curGimbalRoll);}
qreal gimbalPitch () const{ return static_cast<qreal>(_curGimbalPitch); }
qreal gimbalYaw () const{ return static_cast<qreal>(_curGimbalYaw); }
bool gimbalData () const{ return _haveGimbalData; }
bool isROIEnabled () const{ return _isROIEnabled; } bool isROIEnabled () const{ return _isROIEnabled; }
GimbalController* gimbalController () { return _gimbalController; }
CheckList checkListState () { return _checkListState; } CheckList checkListState () { return _checkListState; }
void setCheckListState (CheckList cl) { _checkListState = cl; emit checkListStateChanged(); } void setCheckListState (CheckList cl) { _checkListState = cl; emit checkListStateChanged(); }
@ -1028,10 +1020,6 @@ signals:
void requestProtocolVersion (unsigned version); void requestProtocolVersion (unsigned version);
void mavlinkStatusChanged (); void mavlinkStatusChanged ();
void gimbalRollChanged ();
void gimbalPitchChanged ();
void gimbalYawChanged ();
void gimbalDataChanged ();
void isROIEnabledChanged (); void isROIEnabledChanged ();
void initialConnectComplete (); void initialConnectComplete ();
@ -1217,6 +1205,7 @@ private:
ComponentInformationManager* _componentInformationManager = nullptr; ComponentInformationManager* _componentInformationManager = nullptr;
VehicleObjectAvoidance* _objectAvoidance = nullptr; VehicleObjectAvoidance* _objectAvoidance = nullptr;
Autotune* _autotune = nullptr; Autotune* _autotune = nullptr;
GimbalController* _gimbalController = nullptr;
bool _armed = false; ///< true: vehicle is armed bool _armed = false; ///< true: vehicle is armed
uint8_t _base_mode = 0; ///< base_mode from HEARTBEAT uint8_t _base_mode = 0; ///< base_mode from HEARTBEAT
@ -1257,10 +1246,6 @@ private:
uint8_t _compID = 0; uint8_t _compID = 0;
bool _heardFrom = false; bool _heardFrom = false;
float _curGimbalRoll = 0.0f;
float _curGimbalPitch = 0.0f;
float _curGimbalYaw = 0.0f;
bool _haveGimbalData = false;
bool _isROIEnabled = false; bool _isROIEnabled = false;
Joystick* _activeJoystick = nullptr; Joystick* _activeJoystick = nullptr;

Write Preview
Loading…
Cancel
Save