From db09a1386f3761bb7f5a81ae42bfde24a42a7ab5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 18 Nov 2020 10:03:39 +0100
Subject: [PATCH] vmount: fix regression around stabilize flags

We use the stabilize param to set the input. Like this the input flags
can be changed using the configure message later, and then eventually
used in the output.
---
 src/modules/vmount/input.cpp  |  8 +++++++-
 src/modules/vmount/input.h    |  2 ++
 src/modules/vmount/output.cpp | 11 ++++-------
 src/modules/vmount/output.h   |  2 --
 src/modules/vmount/vmount.cpp | 21 +++++++++++----------
 5 files changed, 24 insertions(+), 20 deletions(-)

diff --git a/src/modules/vmount/input.cpp b/src/modules/vmount/input.cpp
index 2bf3200c38..f6e544076e 100644
--- a/src/modules/vmount/input.cpp
+++ b/src/modules/vmount/input.cpp
@@ -76,5 +76,11 @@ void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude,
 	_control_data.type_data.lonlat.yaw_angle_offset = 0.f;
 }
 
-} /* namespace vmount */
+void InputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
+{
+	_control_data.stabilize_axis[0] = roll_stabilize;
+	_control_data.stabilize_axis[1] = pitch_stabilize;
+	_control_data.stabilize_axis[2] = yaw_stabilize;
+}
 
+} /* namespace vmount */
diff --git a/src/modules/vmount/input.h b/src/modules/vmount/input.h
index 2a7c113219..e7b26a697e 100644
--- a/src/modules/vmount/input.h
+++ b/src/modules/vmount/input.h
@@ -73,6 +73,8 @@ public:
 	/** report status to stdout */
 	virtual void print_status() = 0;
 
+	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
+
 protected:
 	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0;
 
diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp
index b2541d94d7..913274aad0 100644
--- a/src/modules/vmount/output.cpp
+++ b/src/modules/vmount/output.cpp
@@ -72,13 +72,6 @@ void OutputBase::publish()
 	_mount_orientation_pub.publish(mount_orientation);
 }
 
-void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
-{
-	_stabilize[0] = roll_stabilize;
-	_stabilize[1] = pitch_stabilize;
-	_stabilize[2] = yaw_stabilize;
-}
-
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 				   const vehicle_global_position_s &global_position)
 {
@@ -142,6 +135,10 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 		_angle_velocity[2] = NAN;
 		break;
 	}
+
+	for (int i = 0; i < 3; ++i) {
+		_stabilize[i] = control_data->stabilize_axis[i];
+	}
 }
 
 void OutputBase::_handle_position_update(bool force_update)
diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h
index 1c13edd16e..21e6f09275 100644
--- a/src/modules/vmount/output.h
+++ b/src/modules/vmount/output.h
@@ -97,8 +97,6 @@ public:
 	/** Publish _angle_outputs as a mount_orientation message. */
 	void publish();
 
-	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
-
 protected:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);
diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp
index 2fe8956091..161f3ae9a2 100644
--- a/src/modules/vmount/vmount.cpp
+++ b/src/modules/vmount/vmount.cpp
@@ -345,16 +345,6 @@ static int vmount_thread_main(int argc, char *argv[])
 				thread_should_exit = true;
 				break;
 			}
-
-			if (params.mnt_do_stab == 1) {
-				thread_data.output_obj->set_stabilize(true, true, true);
-
-			} else if (params.mnt_do_stab == 2) {
-				thread_data.output_obj->set_stabilize(false, false, true);
-
-			} else {
-				thread_data.output_obj->set_stabilize(false, false, false);
-			}
 		}
 
 		if (thread_data.input_objs_len > 0) {
@@ -364,6 +354,17 @@ static int vmount_thread_main(int argc, char *argv[])
 
 			for (int i = 0; i < thread_data.input_objs_len; ++i) {
 
+				if (params.mnt_do_stab == 1) {
+					thread_data.input_objs[i]->set_stabilize(true, true, true);
+
+				} else if (params.mnt_do_stab == 2) {
+					thread_data.input_objs[i]->set_stabilize(false, false, true);
+
+				} else {
+					thread_data.input_objs[i]->set_stabilize(false, false, false);
+				}
+
+
 				bool already_active = (last_active == i);
 
 				ControlData *control_data_to_check = nullptr;