vmount: publish mount_status

We need feedback in mavlink about the attitude of the gimbal. Therefore
the gimbal output angles are published in vmount.

src/drivers/vmount/output.cpp

@@ -41,8 +41,10 @@
 #include "output.h"
 #include <errno.h>
 
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/mount_status.h>
 #include <px4_defines.h>
 #include <geo/geo.h>
 #include <math.h>
@@ -81,6 +83,18 @@ int OutputBase::initialize()
 	return 0;
 }
 
+void OutputBase::publish()
+{
+	int instance;
+	mount_status_s mount_status;
+
+	for (unsigned i = 0; i < 3; ++i) {
+		mount_status.attitude_euler_angle[i] = _angle_outputs[i];
+	}
+
+	orb_publish_auto(ORB_ID(mount_status), &_mount_status_pub, &mount_status, &instance, ORB_PRIO_DEFAULT);
+}
+
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 				   const vehicle_global_position_s &global_position)
 {

src/drivers/vmount/output.h

@@ -42,6 +42,7 @@
 #include "common.h"
 #include <drivers/drv_hrt.h>
 #include <geo/geo.h>
+#include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 
 
@@ -79,6 +80,9 @@ public:
 	/** report status to stdout */
 	virtual void print_status() = 0;
 
+	/** Publish _angle_outputs as a mount_status message. */
+	void publish();
+
 protected:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);
@@ -110,6 +114,7 @@ private:
 	int _vehicle_attitude_sub = -1;
 	int _vehicle_global_position_sub = -1;
 
+	orb_advert_t _mount_status_pub = nullptr;
 };
 
 

src/drivers/vmount/vmount.cpp

@@ -292,6 +292,8 @@ static int vmount_thread_main(int argc, char *argv[])
 				break;
 			}
 
+			thread_data.output_obj->publish();
+
 		} else {
 			//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
 			usleep(1e6);