Merge pull request #1726 from mavlink/pr-add-set-speed

action: add API to set current speed

src/integration_tests/action_goto.cpp

@@ -47,12 +47,23 @@ TEST_F(SitlTest, PX4ActionGoto)
 
     action_ret = action->takeoff();
     EXPECT_EQ(action_ret, Action::Result::Success);
-    std::this_thread::sleep_for(std::chrono::seconds(2));
+    std::this_thread::sleep_for(std::chrono::seconds(4));
 
     // Go somewhere
-    action->goto_location(47.398000, 8.545592, NAN, NAN);
-    std::this_thread::sleep_for(std::chrono::seconds(10));
+    action->goto_location(47.4, 8.545592, NAN, NAN);
+    std::this_thread::sleep_for(std::chrono::seconds(4));
+
+    LogInfo() << "Go slow for a bit.";
+    // Go slow for a bit
+    EXPECT_EQ(Action::Result::Success, action->set_current_speed(1.0f));
+    std::this_thread::sleep_for(std::chrono::seconds(4));
+
+    LogInfo() << "Go fast again.";
+    // Then faster again
+    EXPECT_EQ(Action::Result::Success, action->set_current_speed(5.0f));
+    std::this_thread::sleep_for(std::chrono::seconds(4));
 
+    LogInfo() << "Go back.";
     // And back
     action->goto_location(47.3977233, 8.545592, NAN, NAN);
     std::this_thread::sleep_for(std::chrono::seconds(10));

src/mavsdk/plugins/action/action.cpp

@@ -256,6 +256,16 @@ Action::Result Action::set_return_to_launch_altitude(float relative_altitude_m)
     return _impl->set_return_to_launch_altitude(relative_altitude_m);
 }
 
+void Action::set_current_speed_async(float speed_m_s, const ResultCallback callback)
+{
+    _impl->set_current_speed_async(speed_m_s, callback);
+}
+
+Action::Result Action::set_current_speed(float speed_m_s) const
+{
+    return _impl->set_current_speed(speed_m_s);
+}
+
 std::ostream& operator<<(std::ostream& str, Action::Result const& result)
 {
     switch (result) {

src/mavsdk/plugins/action/action_impl.cpp

@@ -687,6 +687,33 @@ std::pair<Action::Result, float> ActionImpl::get_return_to_launch_altitude() con
         result.second);
 }
 
+void ActionImpl::set_current_speed_async(float speed_m_s, const Action::ResultCallback& callback)
+{
+    MavlinkCommandSender::CommandLong command{};
+
+    command.command = MAV_CMD_DO_CHANGE_SPEED;
+    command.params.maybe_param1 = 1.0f; // ground speed
+    command.params.maybe_param2 = speed_m_s;
+    command.params.maybe_param3 = -1.0f; // no throttle set
+    command.params.maybe_param4 = 0.0f; // reserved
+    command.target_component_id = _parent->get_autopilot_id();
+
+    _parent->send_command_async(
+        command, [this, callback](MavlinkCommandSender::Result result, float) {
+            command_result_callback(result, callback);
+        });
+}
+
+Action::Result ActionImpl::set_current_speed(float speed_m_s)
+{
+    auto prom = std::promise<Action::Result>();
+    auto fut = prom.get_future();
+
+    set_current_speed_async(speed_m_s, [&prom](Action::Result result) { prom.set_value(result); });
+
+    return fut.get();
+}
+
 Action::Result ActionImpl::action_result_from_command_result(MavlinkCommandSender::Result result)
 {
     switch (result) {

src/mavsdk/plugins/action/action_impl.h

@@ -94,6 +94,9 @@ class ActionImpl : public PluginImplBase {
     void get_return_to_launch_altitude_async(
         const Action::GetReturnToLaunchAltitudeCallback& callback) const;
 
+    void set_current_speed_async(float speed_m_s, const Action::ResultCallback& callback);
+    Action::Result set_current_speed(float speed_m_s);
+
     Action::Result set_return_to_launch_altitude(const float relative_altitude_m) const;
     std::pair<Action::Result, float> get_return_to_launch_altitude() const;
 

src/mavsdk/plugins/action/include/plugins/action/action.h

@@ -587,6 +587,28 @@ class Action : public PluginBase {
      */
     Result set_return_to_launch_altitude(float relative_altitude_m) const;
 
+    /**
+     * @brief Set current speed.
+     *
+     * This will set the speed during a mission, reposition, and similar.
+     * It is ephemeral, so not stored on the drone and does not survive a reboot.
+     *
+     * This function is non-blocking. See 'set_current_speed' for the blocking counterpart.
+     */
+    void set_current_speed_async(float speed_m_s, const ResultCallback callback);
+
+    /**
+     * @brief Set current speed.
+     *
+     * This will set the speed during a mission, reposition, and similar.
+     * It is ephemeral, so not stored on the drone and does not survive a reboot.
+     *
+     * This function is blocking. See 'set_current_speed_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    Result set_current_speed(float speed_m_s) const;
+
     /**
      * @brief Copy constructor.
      */

src/mavsdk/plugins/action/mocks/action_mock.h

@@ -32,6 +32,7 @@ class MockAction {
     MOCK_CONST_METHOD1(set_maximum_speed, Action::Result(float)){};
     MOCK_CONST_METHOD0(get_return_to_launch_altitude, std::pair<Action::Result, float>()){};
     MOCK_CONST_METHOD1(set_return_to_launch_altitude, Action::Result(float)){};
+    MOCK_CONST_METHOD1(set_current_speed, Action::Result(float)){};
 };
 
 } // namespace testing