Hardware Configuration Manager

.gitignore

@@ -13,3 +13,5 @@
 *CMakeFiles*
 *catkin_generated*
 *Makefile
+.idea/
+cmake-build-*/

rosflight/CMakeLists.txt

@@ -64,6 +64,7 @@ add_library(mavrosflight
   src/mavrosflight/param_manager.cpp
   src/mavrosflight/param.cpp
   src/mavrosflight/time_manager.cpp
+  src/mavrosflight/config_manager.cpp
 )
 add_dependencies(mavrosflight ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(mavrosflight

rosflight/include/rosflight/mavrosflight/config_manager.h

@@ -0,0 +1,76 @@
+#include <vector>
+#include <future>
+#include <chrono>
+#include <string>
+#include <utility>
+
+#include "mavlink_listener_interface.h"
+#include "mavlink_comm.h"
+
+namespace mavrosflight
+{
+
+  class ConfigManager : public MavlinkListenerInterface
+  {
+  public:
+    typedef struct
+    {
+      bool successful;
+      bool reboot_required;
+      std::string error_message;
+    } config_response_t;
+    ConfigManager(MavlinkComm *const comm);
+    ~ConfigManager();
+    void handle_mavlink_message(const mavlink_message_t &msg) override;
+    std::tuple<bool, uint8_t> get_configuration(uint8_t device);
+    config_response_t set_configuration(uint8_t device, uint8_t config);
+    void request_config_info();
+
+    std::tuple<bool, uint8_t> get_device_from_str(const std::string &name) const;
+    std::tuple<bool, uint8_t> get_config_from_str(uint8_t device, const std::string &name) const;
+    bool is_valid_device(uint8_t device) const;
+    bool is_valid_config(uint8_t device, uint8_t config) const;
+    std::string get_device_name(uint8_t device) const;
+    std::string get_config_name(uint8_t device, uint8_t config) const;
+
+    static constexpr std::chrono::milliseconds timeout{500};
+  private:
+    typedef struct
+    {
+      std::promise<uint8_t> promise;
+      uint8_t device;
+    } config_promise_t;
+    MavlinkComm *const comm_;
+    std::vector<config_promise_t *> promises_;
+
+
+    typedef struct
+    {
+      std::promise<config_response_t> promise;
+      uint8_t device;
+      uint8_t config;
+    } config_response_promise_t;
+    std::vector<config_response_promise_t *> config_response_promises_;
+
+    typedef struct
+    {
+      std::string name;
+      std::string internal_name;
+      uint8_t max_value;
+      std::vector<std::string> config_names;
+    } device_info_t;
+    std::vector<device_info_t> device_info_;
+
+    void handle_config_message(const mavlink_message_t &msg);
+    void handle_device_info_message(const mavlink_message_t &msg);
+    void handle_config_info_message(const mavlink_message_t &msg);
+    void handle_config_response_message(const mavlink_message_t &msg);
+    void send_config_get_request(uint8_t device);
+    void send_config_set_request(uint8_t device, uint8_t config);
+
+    static std::string make_internal_name(const std::string &name);
+    static std::vector<std::string> get_words(const std::string &internal_name);
+    static bool is_uint8(const std::string &str);
+
+  }; // class ConfigManager
+} // namespace mavrosflight

rosflight/include/rosflight/mavrosflight/mavrosflight.h

@@ -41,6 +41,7 @@
 #include <rosflight/mavrosflight/mavlink_comm.h>
 #include <rosflight/mavrosflight/param_manager.h>
 #include <rosflight/mavrosflight/time_manager.h>
+#include <rosflight/mavrosflight/config_manager.h>
 
 #include <rosflight/mavrosflight/mavlink_listener_interface.h>
 #include <rosflight/mavrosflight/param_listener_interface.h>
@@ -73,6 +74,7 @@ class MavROSflight
   MavlinkComm& comm;
   ParamManager param;
   TimeManager time;
+  ConfigManager config_manager;
 
 private:
 

rosflight/include/rosflight/rosflight_io.h

@@ -75,6 +75,8 @@
 #include <rosflight_msgs/ParamFile.h>
 #include <rosflight_msgs/ParamGet.h>
 #include <rosflight_msgs/ParamSet.h>
+#include <rosflight_msgs/ConfigSet.h>
+#include <rosflight_msgs/ConfigGet.h>
 
 #include <rosflight/mavrosflight/mavrosflight.h>
 #include <rosflight/mavrosflight/mavlink_comm.h>
@@ -144,6 +146,8 @@ class rosflightIO :
   bool calibrateAirspeedSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
   bool rebootSrvCallback(std_srvs::Trigger::Request & req, std_srvs::Trigger::Response &res);
   bool rebootToBootloaderSrvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
+  bool configGetSrvCallback(rosflight_msgs::ConfigGet::Request &req, rosflight_msgs::ConfigGet::Response &res);
+  bool configSetSrvCallback(rosflight_msgs::ConfigSet::Request &req, rosflight_msgs::ConfigSet::Response &res);
 
   // timer callbacks
   void paramTimerCallback(const ros::TimerEvent &e);
@@ -204,6 +208,8 @@ class rosflightIO :
   ros::ServiceServer calibrate_airspeed_srv_;
   ros::ServiceServer reboot_srv_;
   ros::ServiceServer reboot_bootloader_srv_;
+  ros::ServiceServer config_get_srv_;
+  ros::ServiceServer config_set_srv_;
 
   ros::Timer param_timer_;
   ros::Timer version_timer_;