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 #include #include +#include #include #include #include @@ -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) {