From fc89705ef848a3482e2112ab75a1fb4629d5d8c6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 1 Nov 2021 17:54:52 -0700 Subject: [PATCH] navigator: stop capturing images on mission exit When a mission starts capturing images using the MAV_CMD_IMAGE_START_CAPTURE command, it should also stop it again, in case it is stopped early, e.g. with RTL. --- src/modules/navigator/mission.cpp | 1 + src/modules/navigator/navigator.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 27 ++++++++++++++++++++++++ 3 files changed, 32 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8409272daa..aefc19fef7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -173,6 +173,7 @@ Mission::on_inactivation() cmd.param3 = 1.0f; _navigator->publish_vehicle_cmd(&cmd); + _navigator->stop_capturing_images(); _navigator->release_gimbal_control(); if (_navigator->get_precland()->is_activated()) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 31fd91e1e0..b0e84064f0 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -329,6 +329,7 @@ public: void release_gimbal_control(); void calculate_breaking_stop(double &lat, double &lon, float &yaw); + void stop_capturing_images(); private: @@ -422,6 +423,9 @@ private: traffic_buffer_s _traffic_buffer{}; + bool _is_capturing_images{false}; // keep track if we need to stop capturing images + + // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c03a82198d..380052ba7a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1499,7 +1499,20 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) // sent to the mavlink links to other components. switch (vcmd->command) { case NAV_CMD_IMAGE_START_CAPTURE: + + // We are only capturing multiple if param3 is 0 or > 1. + if (static_cast(vcmd->param3) != 1) { + _is_capturing_images = true; + } + + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + break; + case NAV_CMD_IMAGE_STOP_CAPTURE: + _is_capturing_images = false; + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + break; + case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: vcmd->target_component = 100; // MAV_COMP_ID_CAMERA @@ -1552,6 +1565,20 @@ void Navigator::release_gimbal_control() publish_vehicle_cmd(&vcmd); } + +void +Navigator::stop_capturing_images() +{ + if (_is_capturing_images) { + vehicle_command_s vcmd = {}; + vcmd.command = NAV_CMD_IMAGE_STOP_CAPTURE; + vcmd.param1 = 0.0f; + publish_vehicle_cmd(&vcmd); + + // _is_capturing_images is reset inside publish_vehicle_cmd. + } +} + bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) { if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&