Commit e28abddc authored by Gus Grubba's avatar Gus Grubba

Map camera controls

Map gimbal controls
Keep track of gimbal position within Vehicle
parent 089860de
...@@ -378,15 +378,63 @@ QGCCameraManager::_activeJoystickChanged(Joystick* joystick) ...@@ -378,15 +378,63 @@ QGCCameraManager::_activeJoystickChanged(Joystick* joystick)
{ {
qCDebug(CameraManagerLog) << "Joystick changed"; qCDebug(CameraManagerLog) << "Joystick changed";
if(_activeJoystick) { if(_activeJoystick) {
disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); disconnect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom);
disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); disconnect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera);
disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); disconnect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream);
disconnect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera);
disconnect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording);
disconnect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording);
disconnect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording);
} }
_activeJoystick = joystick; _activeJoystick = joystick;
if(_activeJoystick) { if(_activeJoystick) {
connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom); connect(_activeJoystick, &Joystick::stepZoom, this, &QGCCameraManager::_stepZoom);
connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera); connect(_activeJoystick, &Joystick::stepCamera, this, &QGCCameraManager::_stepCamera);
connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream); connect(_activeJoystick, &Joystick::stepStream, this, &QGCCameraManager::_stepStream);
connect(_activeJoystick, &Joystick::triggerCamera, this, &QGCCameraManager::_triggerCamera);
connect(_activeJoystick, &Joystick::startVideoRecord, this, &QGCCameraManager::_startVideoRecording);
connect(_activeJoystick, &Joystick::stopVideoRecord, this, &QGCCameraManager::_stopVideoRecording);
connect(_activeJoystick, &Joystick::toggleVideoRecord, this, &QGCCameraManager::_toggleVideoRecording);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_triggerCamera()
{
QGCCameraControl* pCamera = currentCameraInstance();
if(pCamera) {
pCamera->takePhoto();
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_startVideoRecording()
{
QGCCameraControl* pCamera = currentCameraInstance();
if(pCamera) {
pCamera->startVideo();
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_stopVideoRecording()
{
QGCCameraControl* pCamera = currentCameraInstance();
if(pCamera) {
pCamera->stopVideo();
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_toggleVideoRecording()
{
QGCCameraControl* pCamera = currentCameraInstance();
if(pCamera) {
pCamera->toggleVideo();
} }
} }
......
...@@ -60,6 +60,10 @@ protected slots: ...@@ -60,6 +60,10 @@ protected slots:
virtual void _stepCamera (int direction); virtual void _stepCamera (int direction);
virtual void _stepStream (int direction); virtual void _stepStream (int direction);
virtual void _cameraTimeout (); virtual void _cameraTimeout ();
virtual void _triggerCamera ();
virtual void _startVideoRecording ();
virtual void _stopVideoRecording ();
virtual void _toggleVideoRecording ();
protected: protected:
virtual QGCCameraControl* _findCamera (int id); virtual QGCCameraControl* _findCamera (int id);
......
...@@ -40,6 +40,7 @@ const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; ...@@ -40,6 +40,7 @@ const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine";
const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm");
const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm");
const char* Joystick::_buttonActionToggleArm = QT_TR_NOOP("Toggle Arm");
const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); const char* Joystick::_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing");
const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); const char* Joystick::_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor");
const char* Joystick::_buttonActionZoomIn = QT_TR_NOOP("Zoom In"); const char* Joystick::_buttonActionZoomIn = QT_TR_NOOP("Zoom In");
...@@ -52,6 +53,11 @@ const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Came ...@@ -52,6 +53,11 @@ const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Came
const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video");
const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video");
const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video");
const char* Joystick::_buttonActionGimbalDown = QT_TR_NOOP("Gimbal Down");
const char* Joystick::_buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up");
const char* Joystick::_buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left");
const char* Joystick::_buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right");
const char* Joystick::_buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center");
const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
"RollAxis", "RollAxis",
...@@ -482,9 +488,9 @@ void Joystick::run(void) ...@@ -482,9 +488,9 @@ void Joystick::run(void)
throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f));
} }
if ( _exponential != 0 ) { if ( _exponential < -0.01f) {
// Exponential (0% to -50% range like most RC radios) // Exponential (0% to -50% range like most RC radios)
//_exponential is set by a slider in joystickConfig.qml //_exponential is set by a slider in joystickConfigAdvanced.qml
// Calculate new RPY with exponential applied // Calculate new RPY with exponential applied
roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll;
...@@ -507,7 +513,7 @@ void Joystick::run(void) ...@@ -507,7 +513,7 @@ void Joystick::run(void)
quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
quint16 buttonBit = 1 << buttonIndex; quint16 buttonBit = static_cast<quint16>(1 << buttonIndex);
if (!_rgButtonValues[buttonIndex]) { if (!_rgButtonValues[buttonIndex]) {
// Button up, just record it // Button up, just record it
...@@ -528,9 +534,9 @@ void Joystick::run(void) ...@@ -528,9 +534,9 @@ void Joystick::run(void)
} }
_lastButtonBits = newButtonBits; _lastButtonBits = newButtonBits;
qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle; qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw;
// NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub. // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub.
emit manualControl(roll, -pitch, yaw, throttle, gimbalPitch, gimbalYaw, buttonPressedBits, _activeVehicle->joystickMode()); emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode());
} }
// Sleep. Update rate of joystick is by default 25 Hz // Sleep. Update rate of joystick is by default 25 Hz
...@@ -643,7 +649,7 @@ int Joystick::getFunctionAxis(AxisFunction_t function) ...@@ -643,7 +649,7 @@ int Joystick::getFunctionAxis(AxisFunction_t function)
QStringList Joystick::actions(void) QStringList Joystick::actions(void)
{ {
QStringList list; QStringList list;
list << _buttonActionArm << _buttonActionDisarm; list << _buttonActionArm << _buttonActionDisarm << _buttonActionToggleArm;
if (_activeVehicle) { if (_activeVehicle) {
list << _activeVehicle->flightModes(); list << _activeVehicle->flightModes();
} }
...@@ -655,6 +661,11 @@ QStringList Joystick::actions(void) ...@@ -655,6 +661,11 @@ QStringList Joystick::actions(void)
list << _buttonActionStartVideoRecord; list << _buttonActionStartVideoRecord;
list << _buttonActionStopVideoRecord; list << _buttonActionStopVideoRecord;
list << _buttonActionToggleVideoRecord; list << _buttonActionToggleVideoRecord;
list << _buttonActionGimbalDown;
list << _buttonActionGimbalUp;
list << _buttonActionGimbalLeft;
list << _buttonActionGimbalRight;
list << _buttonActionGimbalCenter;
return list; return list;
} }
...@@ -808,6 +819,8 @@ void Joystick::_buttonAction(const QString& action) ...@@ -808,6 +819,8 @@ void Joystick::_buttonAction(const QString& action)
_activeVehicle->setArmed(true); _activeVehicle->setArmed(true);
} else if (action == _buttonActionDisarm) { } else if (action == _buttonActionDisarm) {
_activeVehicle->setArmed(false); _activeVehicle->setArmed(false);
} else if (action == _buttonActionToggleArm) {
_activeVehicle->setArmed(!_activeVehicle->armed());
} else if (action == _buttonActionVTOLFixedWing) { } else if (action == _buttonActionVTOLFixedWing) {
_activeVehicle->setVtolInFwdFlight(true); _activeVehicle->setVtolInFwdFlight(true);
} else if (action == _buttonActionVTOLMultiRotor) { } else if (action == _buttonActionVTOLMultiRotor) {
...@@ -828,6 +841,16 @@ void Joystick::_buttonAction(const QString& action) ...@@ -828,6 +841,16 @@ void Joystick::_buttonAction(const QString& action)
emit stopVideoRecord(); emit stopVideoRecord();
} else if(action == _buttonActionToggleVideoRecord) { } else if(action == _buttonActionToggleVideoRecord) {
emit toggleVideoRecord(); emit toggleVideoRecord();
} else if(action == _buttonActionGimbalUp) {
_activeVehicle->gimbalPitchStep(1);
} else if(action == _buttonActionGimbalDown) {
_activeVehicle->gimbalPitchStep(-1);
} else if(action == _buttonActionGimbalLeft) {
_activeVehicle->gimbalYawStep(-1);
} else if(action == _buttonActionGimbalRight) {
_activeVehicle->gimbalYawStep(1);
} else if(action == _buttonActionGimbalCenter) {
_activeVehicle->centerGimbal();
} else { } else {
qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
} }
......
...@@ -161,10 +161,8 @@ signals: ...@@ -161,10 +161,8 @@ signals:
/// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up
/// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right /// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right
/// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
/// @param gimbalPitch Range is -1:1
/// @param gimbalYaw Range is -1:1
/// @param mode See Vehicle::JoystickMode_t enum /// @param mode See Vehicle::JoystickMode_t enum
void manualControl (float roll, float pitch, float yaw, float throttle, float gimbalPitch, float gimbalYaw, quint16 buttons, int joystickMmode); void manualControl (float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode);
void buttonActionTriggered(int action); void buttonActionTriggered(int action);
...@@ -259,6 +257,7 @@ private: ...@@ -259,6 +257,7 @@ private:
static const char* _buttonActionArm; static const char* _buttonActionArm;
static const char* _buttonActionDisarm; static const char* _buttonActionDisarm;
static const char* _buttonActionToggleArm;
static const char* _buttonActionVTOLFixedWing; static const char* _buttonActionVTOLFixedWing;
static const char* _buttonActionVTOLMultiRotor; static const char* _buttonActionVTOLMultiRotor;
static const char* _buttonActionZoomIn; static const char* _buttonActionZoomIn;
...@@ -271,6 +270,11 @@ private: ...@@ -271,6 +270,11 @@ private:
static const char* _buttonActionStartVideoRecord; static const char* _buttonActionStartVideoRecord;
static const char* _buttonActionStopVideoRecord; static const char* _buttonActionStopVideoRecord;
static const char* _buttonActionToggleVideoRecord; static const char* _buttonActionToggleVideoRecord;
static const char* _buttonActionGimbalDown;
static const char* _buttonActionGimbalUp;
static const char* _buttonActionGimbalLeft;
static const char* _buttonActionGimbalRight;
static const char* _buttonActionGimbalCenter;
private slots: private slots:
void _activeVehicleChanged(Vehicle* activeVehicle); void _activeVehicleChanged(Vehicle* activeVehicle);
......
...@@ -809,6 +809,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes ...@@ -809,6 +809,9 @@ 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_SERIAL_CONTROL: case MAVLINK_MSG_ID_SERIAL_CONTROL:
{ {
...@@ -2241,18 +2244,13 @@ void Vehicle::_loadSettings(void) ...@@ -2241,18 +2244,13 @@ void Vehicle::_loadSettings(void)
if (!_active) { if (!_active) {
return; return;
} }
QSettings settings; QSettings settings;
settings.beginGroup(QString(_settingsGroup).arg(_id)); settings.beginGroup(QString(_settingsGroup).arg(_id));
bool convertOk; bool convertOk;
_joystickMode = static_cast<JoystickMode_t>(settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk));
_joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
if (!convertOk) { if (!convertOk) {
_joystickMode = JoystickModeRC; _joystickMode = JoystickModeRC;
} }
// Joystick enabled is a global setting so first make sure there are any joysticks connected // Joystick enabled is a global setting so first make sure there are any joysticks connected
if (_toolbox->joystickManager()->joysticks().count()) { if (_toolbox->joystickManager()->joysticks().count()) {
setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool()); setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
...@@ -2741,7 +2739,7 @@ void Vehicle::_remoteControlRSSIChanged(uint8_t rssi) ...@@ -2741,7 +2739,7 @@ void Vehicle::_remoteControlRSSIChanged(uint8_t rssi)
} }
} }
void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust, double gimbalPitch, double gimbalYaw) void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust)
{ {
// The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled // The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled
if ( !_joystickEnabled && !_highLatencyLink) { if ( !_joystickEnabled && !_highLatencyLink) {
...@@ -2750,8 +2748,6 @@ void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, ...@@ -2750,8 +2748,6 @@ void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw,
static_cast<float>(pitch), static_cast<float>(pitch),
static_cast<float>(yaw), static_cast<float>(yaw),
static_cast<float>(thrust), static_cast<float>(thrust),
static_cast<float>(gimbalPitch),
static_cast<float>(gimbalYaw),
0, JoystickModeRC); 0, JoystickModeRC);
} }
} }
...@@ -4009,6 +4005,51 @@ void Vehicle::gimbalControlValue(double pitch, double yaw) ...@@ -4009,6 +4005,51 @@ void Vehicle::gimbalControlValue(double pitch, double yaw)
MAV_MOUNT_MODE_MAVLINK_TARGETING); // MAVLink Roll,Pitch,Yaw MAV_MOUNT_MODE_MAVLINK_TARGETING); // MAVLink Roll,Pitch,Yaw
} }
void Vehicle::gimbalPitchStep(int direction)
{
if(!_haveGimbalData) {
double p = static_cast<double>(_curGimbalPitch + direction);
gimbalControlValue(p, static_cast<double>(_curGinmbalYaw));
}
}
void Vehicle::gimbalYawStep(int direction)
{
if(!_haveGimbalData) {
double y = static_cast<double>(_curGinmbalYaw + direction);
gimbalControlValue(static_cast<double>(_curGinmbalYaw), 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(_curGinmbalYaw - o.yaw) > 0.5f) {
_curGinmbalYaw = o.yaw;
emit gimbalYawChanged();
}
if(!_haveGimbalData) {
_haveGimbalData = true;
emit gimbalDataChanged();
}
}
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
......
...@@ -36,6 +36,7 @@ class UASMessage; ...@@ -36,6 +36,7 @@ class UASMessage;
class SettingsManager; class SettingsManager;
class ADSBVehicle; class ADSBVehicle;
class QGCCameraManager; class QGCCameraManager;
class Joystick;
#if defined(QGC_AIRMAP_ENABLED) #if defined(QGC_AIRMAP_ENABLED)
class AirspaceVehicleManager; class AirspaceVehicleManager;
#endif #endif
...@@ -632,6 +633,10 @@ public: ...@@ -632,6 +633,10 @@ 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(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged)
Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged)
Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged)
// The following properties relate to Orbit status // The following properties relate to Orbit status
Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged) Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged)
...@@ -697,7 +702,7 @@ public: ...@@ -697,7 +702,7 @@ public:
// Called when the message drop-down is invoked to clear current count // Called when the message drop-down is invoked to clear current count
Q_INVOKABLE void resetMessages(); Q_INVOKABLE void resetMessages();
Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust, double gimbalPitch = 0, double gimbalYaw = 0); Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust);
Q_INVOKABLE void disconnectInactiveVehicle(void); Q_INVOKABLE void disconnectInactiveVehicle(void);
/// Command vehicle to return to launch /// Command vehicle to return to launch
...@@ -761,7 +766,10 @@ public: ...@@ -761,7 +766,10 @@ public:
Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning); Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);
Q_INVOKABLE void gimbalControlValue(double pitch, double yaw); Q_INVOKABLE void gimbalControlValue (double pitch, double yaw);
Q_INVOKABLE void gimbalPitchStep (int direction);
Q_INVOKABLE void gimbalYawStep (int direction);
Q_INVOKABLE void centerGimbal ();
#if !defined(NO_ARDUPILOT_DIALECT) #if !defined(NO_ARDUPILOT_DIALECT)
Q_INVOKABLE void flashBootloader(void); Q_INVOKABLE void flashBootloader(void);
...@@ -1088,6 +1096,11 @@ public: ...@@ -1088,6 +1096,11 @@ public:
quint64 mavlinkLossCount () { return _mavlinkLossCount; } /// Total number of lost messages quint64 mavlinkLossCount () { return _mavlinkLossCount; } /// Total number of lost messages
float mavlinkLossPercent () { return _mavlinkLossPercent; } /// Running loss rate float mavlinkLossPercent () { return _mavlinkLossPercent; } /// Running loss rate
qreal gimbalRoll () { return static_cast<qreal>(_curGimbalRoll);}
qreal gimbalPitch () { return static_cast<qreal>(_curGimbalPitch); }
qreal gimbalYaw () { return static_cast<qreal>(_curGinmbalYaw); }
bool gimbalData () { return _haveGimbalData; }
signals: signals:
void allLinksInactive(Vehicle* vehicle); void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate); void coordinateChanged(QGeoCoordinate coordinate);
...@@ -1192,8 +1205,13 @@ signals: ...@@ -1192,8 +1205,13 @@ signals:
void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data); void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
// MAVLink protocol version // MAVLink protocol version
void requestProtocolVersion(unsigned version); void requestProtocolVersion (unsigned version);
void mavlinkStatusChanged(); void mavlinkStatusChanged ();
void gimbalRollChanged ();
void gimbalPitchChanged ();
void gimbalYawChanged ();
void gimbalDataChanged ();
private slots: private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
...@@ -1235,8 +1253,9 @@ private slots: ...@@ -1235,8 +1253,9 @@ private slots:
void _protocolVersionTimeOut(void); void _protocolVersionTimeOut(void);
private: private:
bool _containsLink(LinkInterface* link); bool _containsLink (LinkInterface* link);
void _addLink(LinkInterface* link); void _addLink (LinkInterface* link);
void _joystickChanged (Joystick* joystick);
void _loadSettings(void); void _loadSettings(void);
void _saveSettings(void); void _saveSettings(void);
void _startJoystick(bool start); void _startJoystick(bool start);
...@@ -1273,6 +1292,7 @@ private: ...@@ -1273,6 +1292,7 @@ private:
void _handleStatusText(mavlink_message_t& message, bool longVersion); void _handleStatusText(mavlink_message_t& message, bool longVersion);
void _handleOrbitExecutionStatus(const mavlink_message_t& message); void _handleOrbitExecutionStatus(const mavlink_message_t& message);
void _handleMessageInterval(const mavlink_message_t& message); void _handleMessageInterval(const mavlink_message_t& message);
void _handleGimbalOrientation(const mavlink_message_t& message);
// ArduPilot dialect messages // ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT) #if !defined(NO_ARDUPILOT_DIALECT)
void _handleCameraFeedback(const mavlink_message_t& message); void _handleCameraFeedback(const mavlink_message_t& message);
...@@ -1456,6 +1476,12 @@ private: ...@@ -1456,6 +1476,12 @@ private:
uint8_t _compID; uint8_t _compID;
bool _heardFrom; bool _heardFrom;
float _curGimbalRoll = 0.0f;
float _curGimbalPitch = 0.0f;
float _curGinmbalYaw = 0.0f;
bool _haveGimbalData = false;
Joystick* _activeJoystick = nullptr;
int _firmwareMajorVersion; int _firmwareMajorVersion;
int _firmwareMinorVersion; int _firmwareMinorVersion;
int _firmwarePatchVersion; int _firmwarePatchVersion;
......
...@@ -68,7 +68,7 @@ Item { ...@@ -68,7 +68,7 @@ Item {
} }
QGCComboBox { QGCComboBox {
id: buttonActionCombo id: buttonActionCombo
width: ScreenTools.defaultFontPixelWidth * 20 width: ScreenTools.defaultFontPixelWidth * 26
model: _activeJoystick ? _activeJoystick.actions : 0 model: _activeJoystick ? _activeJoystick.actions : 0
onActivated: _activeJoystick.setButtonAction(modelData, textAt(index)) onActivated: _activeJoystick.setButtonAction(modelData, textAt(index))
Component.onCompleted: currentIndex = find(_activeJoystick.buttonActions[modelData]) Component.onCompleted: currentIndex = find(_activeJoystick.buttonActions[modelData])
......
...@@ -27,6 +27,10 @@ Item { ...@@ -27,6 +27,10 @@ Item {
id: calCol id: calCol
spacing: ScreenTools.defaultFontPixelHeight spacing: ScreenTools.defaultFontPixelHeight
anchors.centerIn: parent anchors.centerIn: parent
Item {
height: 1
width: 1
}
Row { Row {
spacing: ScreenTools.defaultFontPixelWidth * 4 spacing: ScreenTools.defaultFontPixelWidth * 4
anchors.horizontalCenter: parent.horizontalCenter anchors.horizontalCenter: parent.horizontalCenter
...@@ -131,10 +135,6 @@ Item { ...@@ -131,10 +135,6 @@ Item {
Column { Column {
spacing: ScreenTools.defaultFontPixelHeight * 0.5 spacing: ScreenTools.defaultFontPixelHeight * 0.5
anchors.verticalCenter: parent.verticalCenter anchors.verticalCenter: parent.verticalCenter
QGCLabel {
text: qsTr("Axis Monitor")
anchors.horizontalCenter: parent.horizontalCenter
}
Connections { Connections {
target: controller target: controller
onAxisValueChanged: { onAxisValueChanged: {
......
...@@ -509,7 +509,7 @@ void MockLink::_handleManualControl(const mavlink_message_t& msg) ...@@ -509,7 +509,7 @@ void MockLink::_handleManualControl(const mavlink_message_t& msg)
mavlink_manual_control_t manualControl; mavlink_manual_control_t manualControl;
mavlink_msg_manual_control_decode(&msg, &manualControl); mavlink_msg_manual_control_decode(&msg, &manualControl);
qDebug() << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r; qCDebug(MockLinkLog) << "MANUAL_CONTROL" << manualControl.x << manualControl.y << manualControl.z << manualControl.r;
} }
void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat) void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat)
......
...@@ -839,12 +839,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, ...@@ -839,12 +839,8 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
* Set the manual control commands. * Set the manual control commands.
* This can only be done if the system has manual inputs enabled and is armed. * This can only be done if the system has manual inputs enabled and is armed.
*/ */
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, float gimbalPitch, float gimbalYaw, quint16 buttons, int joystickMode) void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
{ {
//-- TODO
Q_UNUSED(gimbalPitch);
Q_UNUSED(gimbalYaw);
if (!_vehicle) { if (!_vehicle) {
return; return;
} }
......
...@@ -263,7 +263,7 @@ public slots: ...@@ -263,7 +263,7 @@ public slots:
#endif #endif
/** @brief Set the values for the manual control of the vehicle */ /** @brief Set the values for the manual control of the vehicle */
void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, float gimbalPitch, float gimbalYaw, quint16 buttons, int joystickMode); void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
/** @brief Set the values for the 6dof manual control of the vehicle */ /** @brief Set the values for the 6dof manual control of the vehicle */
#ifndef __mobile__ #ifndef __mobile__
......
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