Refactoring of the MAVLink parameter interface

.ycm_extra_conf.py

@@ -59,27 +59,12 @@
 '-x',
 'c++',
 '-I',
+'.',
+'-I',
 'core',
 '-I',
 'backend/src',
 '-I',
-'plugins/action',
-'-I',
-'plugins/camera',
-'-I',
-'plugins/follow_me',
-'-I',
-'plugins/gimbal',
-'-I',
-'/plugins/info',
-'-I',
-'/plugins/logging',
-'-I',
-'/plugins/mission',
-'-I',
-'/plugins/offboard',
-'-I',
-'/plugins/telemetry',
 '-isystem',
 'third_party/mavlink/include/mavlink/v2.0',
 '-isystem',

backend/src/plugins/action/action_service_impl.h

@@ -132,8 +132,15 @@ class ActionServiceImpl final : public rpc::action::ActionService::Service {
                                     rpc::action::GetTakeoffAltitudeResponse *response) override
     {
         if (response != nullptr) {
-            auto takeoff_altitude = _action.get_takeoff_altitude_m();
-            response->set_altitude_m(takeoff_altitude);
+            auto result_pair = _action.get_takeoff_altitude();
+
+            auto *rpc_action_result = new rpc::action::ActionResult();
+            rpc_action_result->set_result(
+                static_cast<rpc::action::ActionResult::Result>(result_pair.first));
+            rpc_action_result->set_result_str(action_result_str(result_pair.first));
+
+            response->set_allocated_action_result(rpc_action_result);
+            response->set_altitude(result_pair.second);
         }
 
         return grpc::Status::OK;
@@ -145,7 +152,7 @@ class ActionServiceImpl final : public rpc::action::ActionService::Service {
                        rpc::action::SetTakeoffAltitudeResponse * /* response */) override
     {
         if (request != nullptr) {
-            const auto requested_altitude = request->altitude_m();
+            const auto requested_altitude = request->altitude();
             _action.set_takeoff_altitude(requested_altitude);
         }
 
@@ -157,20 +164,35 @@ class ActionServiceImpl final : public rpc::action::ActionService::Service {
                                  rpc::action::GetMaximumSpeedResponse *response) override
     {
         if (response != nullptr) {
-            auto max_speed = _action.get_max_speed_m_s();
-            response->set_speed_m_s(max_speed);
+            auto result_pair = _action.get_max_speed();
+
+            auto *rpc_action_result = new rpc::action::ActionResult();
+            rpc_action_result->set_result(
+                static_cast<rpc::action::ActionResult::Result>(result_pair.first));
+            rpc_action_result->set_result_str(action_result_str(result_pair.first));
+
+            response->set_allocated_action_result(rpc_action_result);
+            response->set_speed(result_pair.second);
         }
 
         return grpc::Status::OK;
     }
 
     grpc::Status SetMaximumSpeed(grpc::ServerContext * /* context */,
                                  const rpc::action::SetMaximumSpeedRequest *request,
-                                 rpc::action::SetMaximumSpeedResponse * /* response */) override
+                                 rpc::action::SetMaximumSpeedResponse *response) override
     {
         if (request != nullptr) {
-            const auto requested_speed = request->speed_m_s();
-            _action.set_max_speed(requested_speed);
+            const auto requested_speed = request->speed();
+            ActionResult action_result = _action.set_max_speed(requested_speed);
+
+            if (response != nullptr) {
+                auto *rpc_action_result = new rpc::action::ActionResult();
+                rpc_action_result->set_result(
+                    static_cast<rpc::action::ActionResult::Result>(action_result));
+                rpc_action_result->set_result_str(action_result_str(action_result));
+                response->set_allocated_action_result(rpc_action_result);
+            }
         }
 
         return grpc::Status::OK;

backend/test/action_service_impl_test.cpp

@@ -18,8 +18,8 @@ using ActionServiceImpl = dronecode_sdk::backend::ActionServiceImpl<MockAction>;
 using ActionResult = dronecode_sdk::rpc::action::ActionResult;
 using InputPair = std::pair<std::string, dronecode_sdk::ActionResult>;
 
-static constexpr auto ARBITRARY_ALTITUDE = 42.42f;
-static constexpr auto ARBITRARY_SPEED = 8.24f;
+static constexpr float ARBITRARY_ALTITUDE = 42.42f;
+static constexpr float ARBITRARY_SPEED = 8.24f;
 
 std::vector<InputPair> generateInputPairs();
 std::string armAndGetTranslatedResult(dronecode_sdk::ActionResult arm_result);
@@ -257,23 +257,24 @@ TEST_F(ActionServiceImplTest, getTakeoffAltitudeCallsGetter)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
-    EXPECT_CALL(action, get_takeoff_altitude_m()).Times(1);
+    EXPECT_CALL(action, get_takeoff_altitude()).Times(1);
     dronecode_sdk::rpc::action::GetTakeoffAltitudeResponse response;
 
     actionService.GetTakeoffAltitude(nullptr, nullptr, &response);
 }
 
-TEST_F(ActionServiceImplTest, getsCorrectTakeoffAltitude)
+TEST_P(ActionServiceImplTest, getsCorrectTakeoffAltitude)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
-    const float expected_altitude = ARBITRARY_ALTITUDE;
-    ON_CALL(action, get_takeoff_altitude_m()).WillByDefault(Return(expected_altitude));
+    const auto expected_pair = std::make_pair<>(GetParam().second, ARBITRARY_ALTITUDE);
+    ON_CALL(action, get_takeoff_altitude()).WillByDefault(Return(expected_pair));
     dronecode_sdk::rpc::action::GetTakeoffAltitudeResponse response;
 
     actionService.GetTakeoffAltitude(nullptr, nullptr, &response);
 
-    EXPECT_EQ(expected_altitude, response.altitude_m());
+    EXPECT_EQ(GetParam().first, ActionResult::Result_Name(response.action_result().result()));
+    EXPECT_EQ(expected_pair.second, response.altitude());
 }
 
 TEST_F(ActionServiceImplTest, getTakeoffAltitudeDoesNotCrashWithNullResponse)
@@ -302,14 +303,14 @@ TEST_F(ActionServiceImplTest, setTakeoffAltitudeCallsSetter)
     actionService.SetTakeoffAltitude(nullptr, &request, nullptr);
 }
 
-TEST_F(ActionServiceImplTest, setTakeoffAltitudeSetsRightValue)
+TEST_P(ActionServiceImplTest, setTakeoffAltitudeSetsRightValue)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
     float expected_altitude = ARBITRARY_ALTITUDE;
     EXPECT_CALL(action, set_takeoff_altitude(expected_altitude)).Times(1);
     dronecode_sdk::rpc::action::SetTakeoffAltitudeRequest request;
-    request.set_altitude_m(expected_altitude);
+    request.set_altitude(expected_altitude);
 
     actionService.SetTakeoffAltitude(nullptr, &request, nullptr);
 }
@@ -326,31 +327,43 @@ TEST_F(ActionServiceImplTest, getMaxSpeedCallsGetter)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
-    EXPECT_CALL(action, get_max_speed_m_s()).Times(1);
+    EXPECT_CALL(action, get_max_speed()).Times(1);
     dronecode_sdk::rpc::action::GetMaximumSpeedResponse response;
 
     actionService.GetMaximumSpeed(nullptr, nullptr, &response);
 }
 
-TEST_F(ActionServiceImplTest, getMaxSpeedGetsRightValue)
+TEST_P(ActionServiceImplTest, getMaxSpeedGetsRightValue)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
-    const auto expected_max_speed = ARBITRARY_SPEED;
-    ON_CALL(action, get_max_speed_m_s()).WillByDefault(Return(expected_max_speed));
+    const auto expected_pair = std::make_pair<>(GetParam().second, ARBITRARY_SPEED);
+    ON_CALL(action, get_max_speed()).WillByDefault(Return(expected_pair));
     dronecode_sdk::rpc::action::GetMaximumSpeedResponse response;
 
     actionService.GetMaximumSpeed(nullptr, nullptr, &response);
 
-    EXPECT_EQ(expected_max_speed, response.speed_m_s());
+    EXPECT_EQ(GetParam().first, ActionResult::Result_Name(response.action_result().result()));
+    EXPECT_EQ(expected_pair.second, response.speed());
 }
 
 TEST_F(ActionServiceImplTest, setMaxSpeedDoesNotCrashWithNullRequest)
 {
     MockAction action;
     ActionServiceImpl actionService(action);
+    dronecode_sdk::rpc::action::SetMaximumSpeedResponse response;
 
-    actionService.SetMaximumSpeed(nullptr, nullptr, nullptr);
+    actionService.SetMaximumSpeed(nullptr, nullptr, &response);
+}
+
+TEST_F(ActionServiceImplTest, setMaxSpeedDoesNotCrashWithNullResponse)
+{
+    MockAction action;
+    ActionServiceImpl actionService(action);
+    dronecode_sdk::rpc::action::SetMaximumSpeedRequest request;
+    request.set_speed(ARBITRARY_SPEED);
+
+    actionService.SetMaximumSpeed(nullptr, &request, nullptr);
 }
 
 TEST_F(ActionServiceImplTest, setMaxSpeedCallsSetter)
@@ -370,7 +383,7 @@ TEST_F(ActionServiceImplTest, setMaxSpeedSetsRightValue)
     const auto expected_speed = ARBITRARY_SPEED;
     EXPECT_CALL(action, set_max_speed(expected_speed)).Times(1);
     dronecode_sdk::rpc::action::SetMaximumSpeedRequest request;
-    request.set_speed_m_s(expected_speed);
+    request.set_speed(expected_speed);
 
     actionService.SetMaximumSpeed(nullptr, &request, nullptr);
 }
@@ -401,6 +414,8 @@ std::vector<InputPair> generateInputPairs()
     input_pairs.push_back(std::make_pair("NO_VTOL_TRANSITION_SUPPORT",
                                          dronecode_sdk::ActionResult::NO_VTOL_TRANSITION_SUPPORT));
     input_pairs.push_back(std::make_pair("UNKNOWN", dronecode_sdk::ActionResult::UNKNOWN));
+    input_pairs.push_back(
+        std::make_pair("PARAMETER_ERROR", dronecode_sdk::ActionResult::PARAMETER_ERROR));
 
     return input_pairs;
 }

core/mavlink_parameters.cpp

@@ -1,5 +1,6 @@
 #include "mavlink_parameters.h"
 #include "system_impl.h"
+#include <future>
 
 namespace dronecode_sdk {
 
@@ -54,6 +55,16 @@ void MAVLinkParameters::set_param_async(const std::string &name,
     _set_param_queue.push_back(new_work);
 }
 
+bool MAVLinkParameters::set_param(const std::string &name, const ParamValue &value, bool extended)
+{
+    auto prom = std::make_shared<std::promise<bool>>();
+    auto res = prom->get_future();
+
+    set_param_async(name, value, [&prom](bool success) { prom->set_value(success); }, extended);
+
+    return res.get();
+}
+
 void MAVLinkParameters::get_param_async(const std::string &name,
                                         get_param_callback_t callback,
                                         bool extended)
@@ -69,6 +80,15 @@ void MAVLinkParameters::get_param_async(const std::string &name,
         return;
     }
 
+    // Use cached value if available.
+    if (_cache.find(name) != _cache.end()) {
+        if (callback) {
+            callback(true, _cache[name]);
+        }
+        return;
+    }
+
+    // Otherwise push work onto queue.
     GetParamWork new_work;
     new_work.callback = callback;
     new_work.param_name = name;
@@ -77,11 +97,20 @@ void MAVLinkParameters::get_param_async(const std::string &name,
     _get_param_queue.push_back(new_work);
 }
 
-// void MAVLinkParameters::save_async()
-//{
-//    _parent.send_command(MAV_CMD_PREFLIGHT_STORAGE,
-//                          MAVLinkCommands::Params {1.0f, 1.0f, 0.0f, NAN, NAN, NAN, NAN});
-//}
+std::pair<bool, MAVLinkParameters::ParamValue> MAVLinkParameters::get_param(const std::string &name,
+                                                                            bool extended)
+{
+    auto prom = std::make_shared<std::promise<std::pair<bool, MAVLinkParameters::ParamValue>>>();
+    auto res = prom->get_future();
+
+    get_param_async(name,
+                    [&prom](bool success, ParamValue value) {
+                        prom->set_value(std::make_pair<>(success, value));
+                    },
+                    extended);
+
+    return res.get();
+}
 
 void MAVLinkParameters::do_work()
 {
@@ -214,6 +243,11 @@ void MAVLinkParameters::do_work()
     }
 }
 
+void MAVLinkParameters::reset_cache()
+{
+    _cache.clear();
+}
+
 void MAVLinkParameters::process_param_value(const mavlink_message_t &message)
 {
     // LogDebug() << "getting param value";
@@ -231,9 +265,10 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t &message)
         auto work = _get_param_queue.borrow_front();
         if (work) {
             if (strncmp(work->param_name.c_str(), param_value.param_id, PARAM_ID_LEN) == 0) {
+                ParamValue value;
+                value.set_from_mavlink_param_value(param_value);
+                _cache[work->param_name] = value;
                 if (work->callback) {
-                    ParamValue value;
-                    value.set_from_mavlink_param_value(param_value);
                     work->callback(true, value);
                 }
                 _state = State::NONE;
@@ -254,6 +289,7 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t &message)
             // Now it still needs to match the param name
             if (strncmp(work->param_name.c_str(), param_value.param_id, PARAM_ID_LEN) == 0) {
                 // We are done, inform caller and go back to idle
+                _cache[work->param_name] = work->param_value;
                 if (work->callback) {
                     work->callback(true);
                 }
@@ -286,9 +322,10 @@ void MAVLinkParameters::process_param_ext_value(const mavlink_message_t &message
         auto work = _get_param_queue.borrow_front();
         if (work) {
             if (strncmp(work->param_name.c_str(), param_ext_value.param_id, PARAM_ID_LEN) == 0) {
+                ParamValue value;
+                value.set_from_mavlink_param_ext_value(param_ext_value);
+                _cache[work->param_name] = value;
                 if (work->callback) {
-                    ParamValue value;
-                    value.set_from_mavlink_param_ext_value(param_ext_value);
                     work->callback(true, value);
                 }
                 _state = State::NONE;
@@ -311,6 +348,7 @@ void MAVLinkParameters::process_param_ext_value(const mavlink_message_t &message
             if (strncmp(work->param_name.c_str(), param_ext_value.param_id, PARAM_ID_LEN) == 0) {
 
                 // We are done, inform caller and go back to idle
+                _cache[work->param_name] = work->param_value;
                 if (work->callback) {
                     work->callback(true);
                 }
@@ -346,6 +384,7 @@ void MAVLinkParameters::process_param_ext_ack(const mavlink_message_t &message)
         if (strncmp(work->param_name.c_str(), param_ext_ack.param_id, PARAM_ID_LEN) == 0) {
             if (param_ext_ack.param_result == PARAM_ACK_ACCEPTED) {
                 // We are done, inform caller and go back to idle
+                _cache[work->param_name] = work->param_value;
                 if (work->callback) {
                     work->callback(true);
                 }

core/mavlink_parameters.h

@@ -10,6 +10,7 @@
 #include <functional>
 #include <cstring> // for memcpy
 #include <cassert>
+#include <map>
 
 namespace dronecode_sdk {
 
@@ -436,18 +437,23 @@ class MAVLinkParameters {
     };
 
     typedef std::function<void(bool success)> set_param_callback_t;
+
+    bool set_param(const std::string &name, const ParamValue &value, bool extended = false);
+
     void set_param_async(const std::string &name,
                          const ParamValue &value,
                          set_param_callback_t callback,
                          bool extended = false);
 
+    std::pair<bool, ParamValue> get_param(const std::string &name, bool extended);
     typedef std::function<void(bool success, ParamValue value)> get_param_callback_t;
     void
     get_param_async(const std::string &name, get_param_callback_t callback, bool extended = false);
 
-    // void save_async();
     void do_work();
 
+    void reset_cache();
+
     friend std::ostream &operator<<(std::ostream &, const ParamValue &);
 
     // Non-copyable
@@ -487,6 +493,8 @@ class MAVLinkParameters {
     };
     LockedQueue<GetParamWork> _get_param_queue{};
 
+    std::map<std::string, ParamValue> _cache{};
+
     void *_timeout_cookie = nullptr;
 
     // dl_time_t _last_request_time = {};