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

@@ -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 */

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;
 

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)

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);

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;