mission: add delay before camera action

integration_tests/mission.cpp

@@ -17,6 +17,7 @@ static std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
                                                      bool is_fly_through,
                                                      float gimbal_pitch_deg,
                                                      float gimbal_yaw_deg,
+                                                     float camera_action_delay_s,
                                                      MissionItem::CameraAction camera_action);
 
 static void compare_mission_items(const std::shared_ptr<MissionItem> original,
@@ -61,40 +62,46 @@ TEST_F(SitlTest, MissionAddWaypointsAndFly)
                          8.5456490218639658,
                          10.0f, 5.0f, false,
                          20.0f, 60.0f,
+                         NAN,
                          MissionItem::CameraAction::NONE));
 
     mission_items.push_back(
         add_mission_item(47.398241338125118,
                          8.5455360114574432,
                          10.0f, 2.0f, true,
                          0.0f, -60.0f,
+                         5.0f,
                          MissionItem::CameraAction::TAKE_PHOTO));
 
     mission_items.push_back(
         add_mission_item(47.398139363821485, 8.5453846156597137,
                          10.0f, 5.0f, true,
                          -46.0f, 0.0f,
+                         NAN,
                          MissionItem::CameraAction::START_VIDEO));
 
     mission_items.push_back(
         add_mission_item(47.398058617228855,
                          8.5454618036746979,
                          10.0f, 2.0f, false,
                          -90.0f, 30.0f,
+                         NAN,
                          MissionItem::CameraAction::STOP_VIDEO));
 
     mission_items.push_back(
         add_mission_item(47.398100366082858,
                          8.5456969141960144,
                          10.0f, 5.0f, false,
                          -45.0f, -30.0f,
+                         NAN,
                          MissionItem::CameraAction::START_PHOTO_INTERVAL));
 
     mission_items.push_back(
         add_mission_item(47.398001890458097,
                          8.5455576181411743,
                          10.0f, 5.0f, false,
                          0.0f, 0.0f,
+                         NAN,
                          MissionItem::CameraAction::STOP_PHOTO_INTERVAL));
 
     {
@@ -240,6 +247,7 @@ std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
                                               bool is_fly_through,
                                               float gimbal_pitch_deg,
                                               float gimbal_yaw_deg,
+                                              float camera_action_delay_s,
                                               MissionItem::CameraAction camera_action)
 {
     auto new_item = std::make_shared<MissionItem>();
@@ -248,6 +256,7 @@ std::shared_ptr<MissionItem> add_mission_item(double latitude_deg,
     new_item->set_speed(speed_m_s);
     new_item->set_fly_through(is_fly_through);
     new_item->set_gimbal_pitch_and_yaw(gimbal_pitch_deg, gimbal_yaw_deg);
+    new_item->set_camera_action_delay(camera_action_delay_s);
     new_item->set_camera_action(camera_action);
 
     // In order to test setting the interval, add it here.
@@ -280,4 +289,9 @@ void compare_mission_items(const std::shared_ptr<MissionItem> original,
         EXPECT_DOUBLE_EQ(original->get_camera_photo_interval_s(),
                          downloaded->get_camera_photo_interval_s());
     }
+
+    if (std::isfinite(original->get_camera_action_delay_s())) {
+        EXPECT_FLOAT_EQ(original->get_camera_action_delay_s(),
+                        downloaded->get_camera_action_delay_s());
+    }
 }

plugins/mission/mission_impl.cpp

@@ -306,6 +306,12 @@ void MissionImpl::assemble_mavlink_messages()
 {
     _mavlink_mission_item_messages.clear();
 
+    bool last_position_valid = false; // This flag is to protect us from using an invalid x/y.
+    MAV_FRAME last_frame;
+    int32_t last_x;
+    int32_t last_y;
+    float last_z;
+
     unsigned item_i = 0;
     for (auto item : _mission_items) {
 
@@ -335,6 +341,12 @@ void MissionImpl::assemble_mavlink_messages()
                                               mission_item_impl.get_mavlink_z(),
                                               MAV_MISSION_TYPE_MISSION);
 
+            last_position_valid = true; // because we checked is_position_finite
+            last_x = mission_item_impl.get_mavlink_x();
+            last_y = mission_item_impl.get_mavlink_y();
+            last_z = mission_item_impl.get_mavlink_z();
+            last_frame = mission_item_impl.get_mavlink_frame();
+
             _mavlink_mission_item_to_mission_item_indices.insert(
                 std::pair <int, int>
             {static_cast<int>(_mavlink_mission_item_messages.size()), item_i});
@@ -411,6 +423,49 @@ void MissionImpl::assemble_mavlink_messages()
             _mavlink_mission_item_messages.push_back(message_gimbal);
         }
 
+        // FIXME: It is a bit of a hack to set a LOITER_TIME waypoint to add a delay.
+        //        A better solution would be to properly use NAV_DELAY instead. This
+        //        would not require us to keep the last lat/lon.
+        if (std::isfinite(mission_item_impl.get_camera_action_delay_s())) {
+            if (!last_position_valid) {
+                // In the case where we get a delay without a previous position, we will have to
+                // ignore it.
+                LogErr() << "Can't set camera action delay without previous position set.";
+
+            } else {
+
+                // Current is the 0th waypoint
+                uint8_t current = ((_mavlink_mission_item_messages.size() == 0) ? 1 : 0);
+
+                uint8_t autocontinue = 1;
+
+                std::shared_ptr<mavlink_message_t> message_delay(new mavlink_message_t());
+                mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(),
+                                                  _parent->get_own_component_id(),
+                                                  message_delay.get(),
+                                                  _parent->get_target_system_id(),
+                                                  _parent->get_target_component_id(),
+                                                  _mavlink_mission_item_messages.size(),
+                                                  last_frame,
+                                                  MAV_CMD_NAV_LOITER_TIME,
+                                                  current,
+                                                  autocontinue,
+                                                  mission_item_impl.get_camera_action_delay_s(), // loiter time in seconds
+                                                  NAN, // empty
+                                                  0.0f, // radius around waypoint in meters ?
+                                                  0.0f, // loiter at center of waypoint
+                                                  last_x,
+                                                  last_y,
+                                                  last_z,
+                                                  MAV_MISSION_TYPE_MISSION);
+
+                _mavlink_mission_item_to_mission_item_indices.insert(
+                    std::pair <int, int>
+                {static_cast<int>(_mavlink_mission_item_messages.size()), item_i});
+                _mavlink_mission_item_messages.push_back(message_delay);
+            }
+        }
+
         if (mission_item_impl.get_camera_action() != MissionItem::CameraAction::NONE) {
             // There is a camera action that we need to send.
 
@@ -570,6 +625,9 @@ void MissionImpl::assemble_mission_items()
                 result = Mission::Result::UNSUPPORTED;
             }
 
+        } else if (it->command == MAV_CMD_NAV_LOITER_TIME) {
+            new_mission_item->set_camera_action_delay(it->param1);
+
         } else {
             LogErr() << "UNSUPPORTED mission item command (" << it->command << ")";
             result = Mission::Result::UNSUPPORTED;

plugins/mission/mission_item.cpp

@@ -40,6 +40,11 @@ void MissionItem::set_gimbal_pitch_and_yaw(float pitch_deg, float yaw_deg)
     _impl->set_gimbal_pitch_and_yaw(pitch_deg, yaw_deg);
 }
 
+void MissionItem::set_camera_action_delay(float delay_s)
+{
+    _impl->set_camera_action_delay(delay_s);
+}
+
 void MissionItem::set_camera_action(CameraAction action)
 {
     _impl->set_camera_action(action);
@@ -75,6 +80,11 @@ float MissionItem::get_speed_m_s() const
     return _impl->get_speed_m_s();
 }
 
+float MissionItem::get_camera_action_delay_s() const
+{
+    return _impl->get_camera_action_delay_s();
+}
+
 MissionItem::CameraAction MissionItem::get_camera_action() const
 {
     return _impl->get_camera_action();

plugins/mission/mission_item.h

@@ -65,6 +65,16 @@ class MissionItem
      */
     void set_gimbal_pitch_and_yaw(float pitch_deg, float yaw_deg);
 
+    /**
+     * @brief Set a delay before executing camera action.
+     *
+     * This can be used to wait for vehicle to slow down or a gimbal to arrive at the set
+     * orientation.
+     *
+     * @param delay_s The time to wait for in seconds.
+     */
+    void set_camera_action_delay(float delay_s);
+
     /**
      * @brief Possible camera actions at a mission item.
      */
@@ -129,6 +139,13 @@ class MissionItem
      */
     float get_speed_m_s() const;
 
+    /**
+     * @brief Get the delay before executing camera action.
+     *
+     * @return The delay in seconds.
+     */
+    float get_camera_action_delay_s() const;
+
     /**
      * @brief Get the camera action set for this mission item.
      *

plugins/mission/mission_item_impl.cpp

@@ -40,6 +40,11 @@ void MissionItemImpl::set_gimbal_pitch_and_yaw(float pitch_deg, float yaw_deg)
     _gimbal_yaw_deg = yaw_deg;
 }
 
+void MissionItemImpl::set_camera_action_delay(float delay_s)
+{
+    _camera_action_delay_s = delay_s;
+}
+
 void MissionItemImpl::set_camera_action(MissionItem::CameraAction action)
 {
     _camera_action = action;

plugins/mission/mission_item_impl.h

@@ -16,6 +16,7 @@ class MissionItemImpl
     void set_speed(float speed_m_s);
     void set_fly_through(bool fly_through);
     void set_gimbal_pitch_and_yaw(float pitch_deg, float yaw_deg);
+    void set_camera_action_delay(float delay_s);
     void set_camera_action(MissionItem::CameraAction action);
     void set_camera_photo_interval(double interval_s);
 
@@ -26,6 +27,7 @@ class MissionItemImpl
     bool get_fly_through() const { return _fly_through; };
     float get_gimbal_pitch_deg() const { return _gimbal_pitch_deg; }
     float get_gimbal_yaw_deg() const { return _gimbal_yaw_deg; }
+    float get_camera_action_delay_s() const { return _camera_action_delay_s; }
     MissionItem::CameraAction get_camera_action() const { return _camera_action; }
     double get_camera_photo_interval_s() const { return _camera_photo_interval_s; }
 
@@ -49,6 +51,7 @@ class MissionItemImpl
     bool _fly_through = false;
     float _gimbal_pitch_deg = NAN;
     float _gimbal_yaw_deg = NAN;
+    float _camera_action_delay_s = NAN;
     MissionItem::CameraAction _camera_action {MissionItem::CameraAction::NONE};
     double _camera_photo_interval_s = 1.0;
 };