diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index c8ea6555de..38732cab9f 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -108,11 +108,34 @@ int OutputMavlinkV1::update(const ControlData *control_data) _vehicle_command_pub.publish(vehicle_command); + _stream_device_attitude_status(); + _last_update = t; return 0; } +void OutputMavlinkV1::_stream_device_attitude_status() +{ + // This enables the use case where the gimbal v2 protocol is used + // between the ground station and the drone, and the gimbal v1 protocol is + // used between the drone and the gimbal. + gimbal_device_attitude_status_s attitude_status{}; + attitude_status.timestamp = hrt_absolute_time(); + attitude_status.target_system = 0; + attitude_status.target_component = 0; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | + gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; + + matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); + matrix::Quatf q(euler); + q.copyTo(attitude_status.q); + + attitude_status.failure_flags = 0; + _attitude_status_pub.publish(attitude_status); +} + void OutputMavlinkV1::print_status() { PX4_INFO("Output: MAVLink gimbal protocol v1"); diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 89eb4a67c4..d44f48b3e9 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -45,6 +45,7 @@ #include #include #include +#include namespace vmount @@ -64,7 +65,9 @@ public: virtual void print_status(); private: + void _stream_device_attitude_status(); uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; }; class OutputMavlinkV2 : public OutputBase