diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 81ca6f1ba9daa3d23aa8fd8377fa608b57f0b28d..cf12aee9098775b2028774bc555718d376bc1f34 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3977,6 +3977,21 @@ void Vehicle::flashBootloader(void) } #endif +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 +} //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index eb00e19faad6d1db473dae3854335d1e163ac2b3..6c02c4ccf4555daf43d27e1d63702d712f4d33e2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -761,6 +761,8 @@ public: Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning); + Q_INVOKABLE void gimbalControlValue(double pitch, double yaw); + #if !defined(NO_ARDUPILOT_DIALECT) Q_INVOKABLE void flashBootloader(void); #endif