From 178ea132b6c6b1ed7a75cdbc27e46dc4f506c75d Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Wed, 29 Nov 2023 15:42:20 +0100
Subject: [PATCH] Navigator: add set_gimbal_neutral() functionality

To point the gimbal forward eg during landing to reduce chance of damage.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 src/modules/navigator/navigator.h        |  2 ++
 src/modules/navigator/navigator_main.cpp | 12 ++++++++++++
 2 files changed, 14 insertions(+)

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index f48cf30bb9..f52271dd95 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -65,6 +65,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/geofence_result.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
@@ -276,6 +277,7 @@ public:
 
 	void acquire_gimbal_control();
 	void release_gimbal_control();
+	void set_gimbal_neutral();
 
 	void calculate_breaking_stop(double &lat, double &lon, float &yaw);
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 493955a493..ee611e7a02 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1508,6 +1508,18 @@ void Navigator::disable_camera_trigger()
 	publish_vehicle_cmd(&cmd);
 }
 
+void Navigator::set_gimbal_neutral()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	vcmd.param1 = NAN;
+	vcmd.param2 = NAN;
+	vcmd.param3 = NAN;
+	vcmd.param4 = NAN;
+	vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
+	publish_vehicle_cmd(&vcmd);
+}
+
 int Navigator::print_usage(const char *reason)
 {
 	if (reason) {