diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e9270246f4..49fc61f63e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -161,14 +161,7 @@ Mission::on_inactive() void Mission::on_inactivation() { - // Disable camera trigger - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // Pause trigger - cmd.param1 = -1.0f; - cmd.param3 = 1.0f; - _navigator->publish_vehicle_cmd(&cmd); - + _navigator->disable_camera_trigger(); _navigator->stop_capturing_images(); _navigator->release_gimbal_control(); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b62bffe334..9151527752 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -302,8 +302,10 @@ public: void acquire_gimbal_control(); void release_gimbal_control(); - void calculate_breaking_stop(double &lat, double &lon, float &yaw); - void stop_capturing_images(); + void calculate_breaking_stop(double &lat, double &lon, float &yaw); + + void stop_capturing_images(); + void disable_camera_trigger(); void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 49ed2c72d3..4354272590 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1527,6 +1527,18 @@ void Navigator::mode_completed(uint8_t nav_state, uint8_t result) _mode_completed_pub.publish(mode_completed); } + +void Navigator::disable_camera_trigger() +{ + // Disable camera trigger + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + cmd.param3 = 1.0f; + publish_vehicle_cmd(&cmd); +} + int Navigator::print_usage(const char *reason) { if (reason) {