From e2b7b95db313049dcbec7475b730fad70ceefce7 Mon Sep 17 00:00:00 2001
From: Emran Billah <emran.billah@spirirobotics.com>
Date: Tue, 25 Jun 2024 18:14:32 -0300
Subject: [PATCH] Flashing process working ok. Need to add a method to allow
 specific params to be written, passed to the program as a cmd line arg

---
 docker-compose-flash-device.yml   | 21 ++++++++
 docker-compose-test-stadalone.yml |  4 +-
 xbeemav/src/XBeeSetup.cpp         | 89 +++++++++++++------------------
 xbeemav/src/main.cpp              |  4 +-
 4 files changed, 60 insertions(+), 58 deletions(-)
 create mode 100644 docker-compose-flash-device.yml

diff --git a/docker-compose-flash-device.yml b/docker-compose-flash-device.yml
new file mode 100644
index 0000000..dcbf784
--- /dev/null
+++ b/docker-compose-flash-device.yml
@@ -0,0 +1,21 @@
+version: "3.8"
+
+
+
+services:
+
+  xbee-mav:
+    command: rosrun xbee_ros_node xbee_config /dev/ttyUSB0 230400
+    build:
+      context: .
+      dockerfile: Dockerfile
+    devices:
+      - /dev/ttyUSB0:/dev/ttyUSB0
+    environment:
+      - "ROS_MASTER_URI=http://ros-master:11311"
+    networks:
+      - sim_network
+
+networks:
+  sim_network:
+    driver: bridge
diff --git a/docker-compose-test-stadalone.yml b/docker-compose-test-stadalone.yml
index 499709e..f5625f8 100644
--- a/docker-compose-test-stadalone.yml
+++ b/docker-compose-test-stadalone.yml
@@ -26,9 +26,7 @@ services:
       - sim_network
 
   xbee-mav:
-    command: tail -f /dev/null
-    # command: roslaunch xbee_ros_node xbeemav.launch baud:=230400 port:="/dev/ttyUSB0" drone:="master" mode:="swarm"
-    container_name: xbee-mav
+    command: rosrun xbee_ros_node xbee_config /dev/ttyUSB0 230400
     depends_on:
       - ros-master
     build:
diff --git a/xbeemav/src/XBeeSetup.cpp b/xbeemav/src/XBeeSetup.cpp
index f19479b..417e1cd 100644
--- a/xbeemav/src/XBeeSetup.cpp
+++ b/xbeemav/src/XBeeSetup.cpp
@@ -13,58 +13,41 @@ bool setupXBee(const std::string &device_port, const unsigned int baud_rate) {
   XBeeModule xbee_module;
   XMLConfigParser config_parser;
 
-  if (xbee_module.Init_Port(device_port, baud_rate)) {
-    std::thread th_service(&XBeeModule::Run_Service, &xbee_module);
-
-    while (!xbee_module.Is_Connected() &&
-           !xbee_module.Check_Time_Out_Exceeded()) {
-      continue;
-    }
-
-    if (xbee_module.Is_Connected()) {
-      std::cout << "Connected to XBee." << std::endl;
-      std::cout << "Loading Config File..." << std::endl;
-
-      if (config_parser.Load_Config()) {
-        std::cout << "Config Loaded Successfully." << std::endl;
-        std::cout << "Transferring Data..." << std::endl;
-        std::vector<XBee_Parameter_S> *config_parameters =
-            config_parser.Get_Loaded_Parameters();
-
-        for (std::size_t i = 0; i < config_parameters->size(); i++) {
-          std::string current_command;
-          xbee_module.Format_AT_Command(config_parameters->at(i),
-                                        &current_command);
-          xbee_module.Send_Data(current_command);
-        }
-
-        std::string write_command = "ATWR \r";
-        xbee_module.Send_Data(write_command);
-      }
-    }
-
-    th_service.join();
-
-    std::cout << "Exiting AT Command Mode..." << std::endl;
-
-    if (xbee_module.Is_Connected())
-    {
-      xbee_module.Exit_AT_Command_Mode();
-
-      if (config_parser.Is_Config_Loaded_Successfully())
-      {
-        std::cout << "XBee Configured Successfully." << std::endl;
-        return true;
-      }
-      else
-      {
-        std::cout << "XBee Configuration Failed." << std::endl;
-      }
-    }
-    else
-    {
-      std::cout << "XBee Configuration Failed." << std::endl;
-    }
+  // Init port at specified baud rate
+  if (!xbee_module.Init_Port(device_port, baud_rate)) {
+    std::cout << "XBee Configuration Failed. Could not Init_Port." << std::endl;
+    return false;
   }
-  return false;
+
+  // Run xbee flashing process in a separate thread
+  std::thread th_service(&XBeeModule::Run_Service, &xbee_module);
+
+  // Wait till connection to device is established
+  while (!xbee_module.Is_Connected() && !xbee_module.Check_Time_Out_Exceeded()) {
+    continue;
+  }
+
+  // Load external config file
+  if (!config_parser.Load_Config()) {
+    std::cout << "XBee Configuration Failed. Could not config file." << std::endl;
+    return false;
+  }
+
+  // Flash config params to device
+  std::cout << "Connected to XBee. Flashing Config File..." << std::endl;
+  std::vector<XBee_Parameter_S> *config_parameters = config_parser.Get_Loaded_Parameters();
+  for (auto& param : *config_parameters) {
+    std::string current_command = "";
+    xbee_module.Format_AT_Command(param, &current_command);
+    xbee_module.Send_Data(current_command);
+  }
+  std::string write_command = "ATWR \r";
+  xbee_module.Send_Data(write_command);
+
+  // Kill service once device has been flashed
+  th_service.join();
+  xbee_module.Exit_AT_Command_Mode();
+
+  std::cout << "Flashing process complete." << std::endl;
+  return true;
 }
diff --git a/xbeemav/src/main.cpp b/xbeemav/src/main.cpp
index fc6a1b1..1302cf2 100644
--- a/xbeemav/src/main.cpp
+++ b/xbeemav/src/main.cpp
@@ -13,7 +13,7 @@ int main(int argc, char*argv[])
   {
     std::string device_port;
     unsigned int baud_rate = 0;
-    const unsigned int DEFAULT_BAUD_RATE = 9600;
+    const unsigned int DEFAULT_BAUD_RATE = 230400;
     const std::string DEFAULT_DEVICE_PORT = "/dev/ttyUSB0";
 
     if (argc  < 1)
@@ -26,7 +26,7 @@ int main(int argc, char*argv[])
     else
       sscanf(argv[2], "%u", &baud_rate);
 
-    //setupXBee(device_port, baud_rate);
+    setupXBee(device_port, baud_rate);
   }
   catch (const std::exception& e)
   {