gimbal: move gimbal controls to new dedicated topic

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/CMakeLists.txt

@@ -99,6 +99,7 @@ set(msg_files
 	GimbalManagerStatus.msg
 	GpsDump.msg
 	GpsInjectData.msg
+	GimbalControls.msg
 	Gripper.msg
 	HealthReport.msg
 	HeaterStatus.msg

msg/GimbalControls.msg

@@ -0,0 +1,9 @@
+uint64 timestamp			# time since system start (microseconds)
+uint8 INDEX_ROLL = 0
+uint8 INDEX_PITCH = 1
+uint8 INDEX_YAW = 2
+
+uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
+float32[3] control
+
+float32 retract_gimbal

src/lib/mixer_module/functions/FunctionGimbal.hpp

@@ -35,7 +35,7 @@
 
 #include "FunctionProviderBase.hpp"
 
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/gimbal_controls.h>
 
 /**
  * Functions: Gimbal_Roll .. Gimbal_Yaw
@@ -48,18 +48,18 @@ public:
 
 	void update() override
 	{
-		actuator_controls_s actuator_controls;
+		gimbal_controls_s gimbal_controls;
 
-		if (_topic.update(&actuator_controls)) {
-			_data[0] = actuator_controls.control[0];
-			_data[1] = actuator_controls.control[1];
-			_data[2] = actuator_controls.control[2];
+		if (_topic.update(&gimbal_controls)) {
+			_data[0] = gimbal_controls.control[gimbal_controls_s::INDEX_ROLL];
+			_data[1] = gimbal_controls.control[gimbal_controls_s::INDEX_PITCH];
+			_data[2] = gimbal_controls.control[gimbal_controls_s::INDEX_YAW];
 		}
 	}
 
 	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; }
 
 private:
-	uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
+	uORB::Subscription _topic{ORB_ID(gimbal_controls)};
 	float _data[3] { NAN, NAN, NAN };
 };

src/modules/gimbal/output_rc.cpp

@@ -34,7 +34,7 @@
 
 #include "output_rc.h"
 
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/gimbal_controls.h>
 #include <px4_platform_common/defines.h>
 #include <matrix/matrix/math.hpp>
 
@@ -62,25 +62,25 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints)
 
 	_stream_device_attitude_status();
 
-	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
-	actuator_controls_s actuator_controls{};
-	actuator_controls.control[0] = constrain(
-					       (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) *
-					       (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
-					       -1.f, 1.f);
-	actuator_controls.control[1] = constrain(
-					       (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) *
-					       (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))),
-					       -1.f, 1.f);
-	actuator_controls.control[2] = constrain(
-					       (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) *
-					       (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))),
-					       -1.f, 1.f);
-	actuator_controls.control[3] = constrain(
-					       _retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode,
-					       -1.f, 1.f);
-	actuator_controls.timestamp = hrt_absolute_time();
-	_actuator_controls_pub.publish(actuator_controls);
+	// _angle_outputs are in radians, gimbal_controls are in [-1, 1]
+	gimbal_controls_s gimbal_controls{};
+	gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(
+				(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) *
+				(1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
+				-1.f, 1.f);
+	gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(
+				(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) *
+				(1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))),
+				-1.f, 1.f);
+	gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain(
+				(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) *
+				(1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))),
+				-1.f, 1.f);
+	gimbal_controls.retract_gimbal = constrain(
+			_retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode,
+			-1.f, 1.f);
+	gimbal_controls.timestamp = hrt_absolute_time();
+	_gimbal_controls_pub.publish(gimbal_controls);
 
 	_last_update = t;
 }

src/modules/gimbal/output_rc.h

@@ -37,7 +37,7 @@
 #include "output.h"
 
 #include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/gimbal_controls.h>
 #include <uORB/topics/gimbal_device_attitude_status.h>
 
 namespace gimbal
@@ -56,7 +56,7 @@ public:
 private:
 	void _stream_device_attitude_status();
 
-	uORB::Publication <actuator_controls_s>	_actuator_controls_pub{ORB_ID(actuator_controls_2)};
+	uORB::Publication <gimbal_controls_s>	_gimbal_controls_pub{ORB_ID(gimbal_controls)};
 	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
 
 	bool _retract_gimbal = true;

src/modules/logger/logged_topics.cpp

@@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("gimbal_manager_set_attitude", 500);
 	add_optional_topic("generator_status");
 	add_optional_topic("gps_dump");
+	add_optional_topic("gimbal_controls", 200);
 	add_optional_topic("gripper");
 	add_optional_topic("heater_status");
 	add_topic("home_position");