update

arduino_lib/packet_creator.h

@@ -56,14 +56,24 @@ void generate_meta_frame(uint8_t *packet, const char *device_name, const char *p
   strncpy((char *)(packet + 2 + 20 + 64 + 10 + 64 + 10 + 64), serialization_format_03, 10);
 }
 
-void generate_data_frame(uint8_t *packet, const char *packet_description, const char *serialization_format,
+bool generate_data_frame(uint8_t *packet, const char *packet_description, const char *serialization_format,
                          std::vector<SDPData> &data)
 {
+  if (std::string(serialization_format) != get_serialization_format(data))
+  {
+    return false;
+  }
+  if (strlen(serialization_format) != std::vector<SDPData>(data).size())
+  {
+    return false;
+  }
   *(uint16_t *)(packet + 0) = esp_now_ros::Packet::PACKET_TYPE_DATA;
   strncpy((char *)(packet + 2), packet_description, 64);
   strncpy((char *)(packet + 2 + 64), serialization_format, 10);
   auto packet_data_p = packet + 2 + 64 + 10;
-  for (auto it = data.begin(); it != data.end(); ++it)
+  auto it = data.begin();
+  int index_sf = 0;
+  while (it != data.end())
   {
     if (std::holds_alternative<int32_t>(*it))
     {
@@ -88,7 +98,7 @@ void generate_data_frame(uint8_t *packet, const char *packet_description, const
       {
         for (int i = str.size(); i < 64; ++i)
         {
-          *(char *)packet_data_p = '\0';
+          *(char *)(packet_data_p + i) = '\0';
         }
         strncpy((char *)packet_data_p, str.c_str(), str.size());
         packet_data_p += 64;
@@ -97,7 +107,7 @@ void generate_data_frame(uint8_t *packet, const char *packet_description, const
       {
         for (int i = str.size(); i < 16; ++i)
         {
-          *(char *)packet_data_p = '\0';
+          *(char *)(packet_data_p + i) = '\0';
         }
         strncpy((char *)packet_data_p, str.c_str(), str.size());
         packet_data_p += 16;
@@ -108,14 +118,17 @@ void generate_data_frame(uint8_t *packet, const char *packet_description, const
       *(bool *)packet_data_p = std::get<bool>(*it);
       packet_data_p += sizeof(bool);
     }
+    ++it;
+    ++index_sf;
   }
+  return true;
 }
 
-void generate_data_frame(uint8_t *packet, const char *packet_description,
+bool generate_data_frame(uint8_t *packet, const char *packet_description,
                          std::vector<SDPData> &data)
 {
   std::string serialization_format = get_serialization_format(data);
-  generate_data_frame(packet, packet_description, serialization_format.c_str(), data);
+  return generate_data_frame(packet, packet_description, serialization_format.c_str(), data);
 }
 
 /* Version 1 functions */

arduino_lib/sdp/sdp_util.h

@@ -50,22 +50,22 @@ void _OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len)
 
 bool _broadcast_sdp_meta_packet(const SDPInterfaceDescription &packet_description_and_serialization_format)
 {
-    uint8_t buf[240];
-    std::string packet_description = std::get<0>(packet_description_and_serialization_format);
-    std::string serialization_format = std::get<1>(packet_description_and_serialization_format);
+    uint8_t buf[245];
+    const std::string &packet_description = std::get<0>(packet_description_and_serialization_format);
+    const std::string &serialization_format = std::get<1>(packet_description_and_serialization_format);
     generate_meta_frame(buf, _sdp_device_name.c_str(), packet_description.c_str(), serialization_format.c_str(), "", "", "", "");
-    return esp_now_send(_peer_broadcast.peer_addr, buf, sizeof(buf)) == ESP_OK;
+    bool success = esp_now_send(_peer_broadcast.peer_addr, buf, sizeof(buf)) == ESP_OK;
+    return success;
 }
 
 void _meta_frame_broadcast_task(void *parameter)
 {
     for (;;)
     {
         vTaskDelay(pdMS_TO_TICKS(10000));
-
         for (auto &entry : sdp_interface_callbacks)
         {
-            SDPInterfaceDescription packet_description_and_serialization_format = std::get<0>(entry);
+            const SDPInterfaceDescription &packet_description_and_serialization_format = std::get<0>(entry);
             _broadcast_sdp_meta_packet(packet_description_and_serialization_format);
         }
     }
@@ -94,7 +94,7 @@ bool init_sdp(uint8_t *mac_address, const String &device_name)
     {
         return false;
     }
-    xTaskCreate(_meta_frame_broadcast_task, "meta_frame_broadcast_task", 4096, NULL, 1, NULL);
+    xTaskCreate(_meta_frame_broadcast_task, "meta_frame_broadcast_task", 16384, NULL, 1, NULL);
     esp_now_register_recv_cb(_OnDataRecv);
     return true;
 }

sketchbooks/sdp_landmark_information/src/main.cpp

@@ -60,10 +60,13 @@ bool load_config_from_FS(fs::FS &fs, String filename = "/config.json")
 
 void setup()
 {
-  M5.begin(true, false, true, false);
+  M5.begin(true, true, true, false);
   Serial.begin(115200);
   Serial2.begin(115200, SERIAL_8N1, 22, 21);
 
+  // LCD Print
+  M5.Lcd.printf("SDP LANDMARK INFORMATION HOST\n");
+
   // Load config from FS
   SPIFFS.begin();
   SD.begin();
@@ -72,29 +75,32 @@ void setup()
     if (not load_config_from_FS(SPIFFS, "/config.json"))
     {
       Serial.println("Failed to load config file");
-      ESP.restart();
+      M5.lcd.printf("Failed to load config file\n");
+      while (true)
+      {
+        delay(1000);
+      }
     }
   }
 
   // Initialization of SDP
   if (not init_sdp(mac_address, device_name))
   {
     Serial.println("Failed to initialize SDP");
-    ESP.restart();
-  }
-  {
-    Serial.println("Failed to initialize ESP-NOW");
-    ESP.restart();
+    M5.lcd.printf("Failed to initialize SDP\n");
+    while (true)
+    {
+      delay(1000);
+    }
   }
-  Serial.println("ESP NOW Initialized!");
+  Serial.println("SDP Initialized!");
 
   // UWB module
   Serial1.begin(115200, SERIAL_8N1, 16, 17);
   bool result = initUWB(false, uwb_id, Serial1);
   data_for_uwb_data_packet.push_back(SDPData(uwb_id));
 
-  // LCD Print
-  M5.Lcd.printf("SDP SWITCHBOT LIGHT HOST\n");
+  // Display MAC address
   M5.Lcd.printf("Name: %s\n", device_name.c_str());
   M5.Lcd.printf("ADDR: %2x:%2x:%2x:%2x:%2x:%2x\n",
                 mac_address[0], mac_address[1], mac_address[2],

sketchbooks/sdp_sesami_host/data/config.json

@@ -1,8 +1,10 @@
 {
+    "device_name": "SDP Sesami",
     "wifi_ssid": "",
     "wifi_password": "",
     "sesami_device_uuid": "",
     "sesami_secret_key": "",
     "sesami_api_key": "",
-    "uwb_id": -1
+    "uwb_id": -1,
+    "comment": "this is configuration for sdp_sesami_host"
 }