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
Split 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 @@ @@ -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() @@ -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() @@ -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 @@ -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 { @@ -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, @@ -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) @@ -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<float>(qQNaN()), // Empty
static_cast<float>(qQNaN()), // Empty
@ -3062,7 +3075,9 @@ void Vehicle::sendMavCommandIntWithHandler(const MavCmdAckHandlerInfo_t* ackHand @@ -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<double> heights) @@ -4290,69 +4305,6 @@ void Vehicle::_altitudeAboveTerrainReceived(bool success, QList<double> 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<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)
{
mavlink_obstacle_distance_t o;
@ -4396,6 +4348,7 @@ void Vehicle::_handleFenceStatus(const mavlink_message_t& message) @@ -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);

29
src/Vehicle/Vehicle.h
Unescape View File

@ -78,6 +78,7 @@ class LinkManager; @@ -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 @@ -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: @@ -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: @@ -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: @@ -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<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; }
GimbalController* gimbalController () { return _gimbalController; }
CheckList checkListState () { return _checkListState; }
void setCheckListState (CheckList cl) { _checkListState = cl; emit checkListStateChanged(); }
@ -1028,10 +1020,6 @@ signals: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save