From b8d389ca4e9847180e46e8622f926aa057353434 Mon Sep 17 00:00:00 2001
From: Nicolas de Palezieux <ndepal@gmail.com>
Date: Thu, 10 Aug 2017 16:03:36 +0200
Subject: [PATCH] vmount: properly unadvertise uorb topics when stopping or
 re-configuring vmount. Othwerise subsequent advertise calls may fail (only
 ever witnessed for _mount_orientation_pub)

---
 src/drivers/vmount/output.cpp         | 4 ++++
 src/drivers/vmount/output_mavlink.cpp | 7 +++++++
 src/drivers/vmount/output_mavlink.h   | 2 +-
 3 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp
index b63c1319ea..93db1c19f8 100644
--- a/src/drivers/vmount/output.cpp
+++ b/src/drivers/vmount/output.cpp
@@ -68,6 +68,10 @@ OutputBase::~OutputBase()
 	if (_vehicle_global_position_sub >= 0) {
 		orb_unsubscribe(_vehicle_global_position_sub);
 	}
+
+	if (_mount_orientation_pub) {
+		orb_unadvertise(_mount_orientation_pub);
+	}
 }
 
 int OutputBase::initialize()
diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp
index 9897a29da7..8dcd45ef29 100644
--- a/src/drivers/vmount/output_mavlink.cpp
+++ b/src/drivers/vmount/output_mavlink.cpp
@@ -52,6 +52,13 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config)
 {
 }
 
+OutputMavlink::~OutputMavlink()
+{
+	if (_vehicle_command_pub) {
+		orb_unadvertise(_vehicle_command_pub);
+	}
+}
+
 int OutputMavlink::update(const ControlData *control_data)
 {
 	vehicle_command_s vehicle_command = {
diff --git a/src/drivers/vmount/output_mavlink.h b/src/drivers/vmount/output_mavlink.h
index 90d91c68e5..bdcc88de66 100644
--- a/src/drivers/vmount/output_mavlink.h
+++ b/src/drivers/vmount/output_mavlink.h
@@ -56,7 +56,7 @@ class OutputMavlink : public OutputBase
 {
 public:
 	OutputMavlink(const OutputConfig &output_config);
-	virtual ~OutputMavlink() { }
+	~OutputMavlink();
 
 	virtual int update(const ControlData *control_data);