Merge pull request #93 from 708yamaguchi/selected-modes

radxa select the mode of use
iory · Nov 27, 2024 · 060fc40 · 060fc40
2 parents 72e99c3 + cc9b716
commit 060fc40
Show file tree

Hide file tree

Showing 16 changed files with 359 additions and 94 deletions.
diff --git a/bin/display_information.py b/bin/display_information.py
@@ -16,6 +16,7 @@
 from riberry.battery import MP2760BatteryMonitor
 from riberry.battery import PisugarBatteryReader
 from riberry.i2c_base import I2CBase
+from riberry.i2c_base import PacketType
 from riberry.network import get_ip_address
 from riberry.network import get_mac_address
 from riberry.network import get_ros_master_ip
@@ -203,9 +204,8 @@ def display_image(self, img):
         jpg_size = len(jpg_img)
 
         header = []
-        header += [0xFF, 0xD8, 0xEA]
+        header += [PacketType.JPEG]
         header += [(jpg_size & 0xFF00) >> 8, (jpg_size & 0x00FF) >> 0]
-
         packer = WirePacker(buffer_size=1000)
         for h in header:
             packer.write(h)
@@ -218,8 +218,7 @@ def display_image(self, img):
 
         for pack in nsplit(jpg_img, n=50):
             packer.reset()
-            for h in [0xFF, 0xD8, 0xEA]:
-                packer.write(h)
+            packer.write(PacketType.JPEG)
             for h in pack:
                 packer.write(h)
             packer.end()
@@ -261,7 +260,8 @@ def display_information(self):
                     battery_str += "-"
                 else:
                     battery_str += "?"
-        sent_str = f"{ip_str}\n{master_str}\n{battery_str}\n"
+        sent_str = [chr(PacketType.TEXT)]
+        sent_str += f"{ip_str}\n{master_str}\n{battery_str}\n"
 
         if ros_available and ros_additional_message:
             sent_str += f"{ros_additional_message}\n"
@@ -270,7 +270,7 @@ def display_information(self):
         self.send_string(sent_str)
 
     def display_qrcode(self, target_url=None):
-        header = [0x02]
+        header = [PacketType.QR_CODE]
         if target_url is None:
             ip = get_ip_address()
             if ip is None:
@@ -284,7 +284,7 @@ def display_qrcode(self, target_url=None):
         self.send_raw_bytes(header)
 
     def force_mode(self, mode_name):
-        header = [0xFF, 0xFE, 0xFD]
+        header = [PacketType.FORCE_MODE]
         forceModebytes = (list (map(ord, mode_name)))
         self.send_raw_bytes(header + forceModebytes)
 

diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_button/atom_s3_button.cpp b/firmware/atom_s3_i2c_display/lib/atom_s3_button/atom_s3_button.cpp
@@ -3,7 +3,6 @@
 AtomS3Button::AtomS3Button(int pin, bool activeLow, bool pullupActive)
   : btn(pin, activeLow, pullupActive), currentButtonState(RESET), clicked(false), doubleClicked(false), longPressed(false) {
   begin();
-  createTask(0);
 }
 
 void AtomS3Button::begin() {

diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.cpp b/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.cpp
@@ -2,12 +2,12 @@
 
 AtomS3I2C* AtomS3I2C::instance = nullptr;
 String AtomS3I2C::requestStr = ""; // Initialize the static requestStr
-String AtomS3I2C::forcedMode = ""; // Initialize the static requestStr
+String AtomS3I2C::forcedMode = ""; // Initialize the static forcedMode
+String AtomS3I2C::selectedModesStr = ""; // Initialize the static selectedModesStr
 
 AtomS3I2C::AtomS3I2C(AtomS3LCD &lcd, AtomS3Button &button)
   : atoms3lcd(lcd), atoms3button(button), receiveEventEnabled(true) {
   instance = this;
-  createTask(0);
 }
 
 void AtomS3I2C::updateLastReceiveTime() {
@@ -18,7 +18,27 @@ bool AtomS3I2C::checkTimeout() {
   return millis() - lastReceiveTime > receiveTimeout;
 }
 
-// Receive data over I2C and store it in the atoms3lcd instance (color_str, jpegBuf, qrCodeData)
+// カンマ区切りの文字列を分割して配列に格納する関数
+int AtomS3I2C::splitString(const String &input, char delimiter, String output[], int maxParts) {
+  int start = 0;
+  int index = 0;
+  while (true) {
+    int end = input.indexOf(delimiter, start);
+    if (end == -1) { // 区切り文字が見つからない場合
+      if (index < maxParts) {
+        output[index++] = input.substring(start); // 最後の部分を追加
+      }
+      break;
+    }
+    if (index < maxParts) {
+      output[index++] = input.substring(start, end);
+    }
+    start = end + 1; // 次の部分へ進む
+  }
+  return index; // 分割された部分の数を返す
+}
+
+// Receive data over I2C and store it in the atoms3lcd instance
 // No rendering is done; lcd update is handled by other tasks
 void AtomS3I2C::receiveEvent(int howMany) {
     if (instance->receiveEventEnabled == false)
@@ -29,47 +49,98 @@ void AtomS3I2C::receiveEvent(int howMany) {
     String str;
     // Read data from the I2C bus
     while (0 < WireSlave.available()) {
-        char c = WireSlave.read();  // receive byte as a character
-        // If currently loading JPEG, store the data in jpegBuf
-        if (instance->atoms3lcd.loadingJpeg && str.length() >= 3) {
-            instance->atoms3lcd.jpegBuf[instance->atoms3lcd.currentJpegIndex + str.length() - 3] = c;
-        }
+        char c = WireSlave.read();  // receive byte as a character;
         str += c;
     }
-    // Check for JPEG packet header and update length if found
-    if (instance->atoms3lcd.readyJpeg == false
-        && str.length() == 5 && (str[0] == jpegPacketHeader[0]) && (str[1] == jpegPacketHeader[1]) && (str[2] == jpegPacketHeader[2])) {
-        instance->atoms3lcd.jpegLength = (uint32_t)(str[3] << 8) | str[4];
+
+    // Parse the first byte as PacketType
+    if (str.length() < 1) {
+        return;  // Invalid packet
+    }
+
+    PacketType packetType = static_cast<PacketType>(str[0]);
+    switch (packetType) {
+        case JPEG:
+            handleJpegPacket(str.substring(1));
+            break;
+
+        case QR_CODE:
+            handleQrCodePacket(str);
+            break;
+
+        case FORCE_MODE:
+            handleForceModePacket(str);
+            break;
+
+        case SELECTED_MODE:
+            handleSelectedModePacket(str);
+            break;
+
+        case DISPLAY_BATTERY_GRAPH_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case SERVO_CONTROL_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case PRESSURE_CONTROL_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        case TEACHING_MODE:
+            instance->atoms3lcd.color_str = str.substring(1); // remove PacketType Header
+            break;
+
+        default:
+            // Handle TEXT or unknown packets
+            instance->atoms3lcd.color_str = str;
+            break;
+    }
+}
+
+void AtomS3I2C::handleJpegPacket(const String& str) {
+    if (str.length() == 2 && !instance->atoms3lcd.readyJpeg) {
+        // Initialize JPEG loading
+        instance->atoms3lcd.jpegLength = (static_cast<uint32_t>(str[0]) << 8) | static_cast<uint8_t>(str[1]);
         instance->atoms3lcd.currentJpegIndex = 0;
         instance->atoms3lcd.loadingJpeg = true;
-        return;
-    }
-    // Continue receiving JPEG data if already in loading state
-    else if (instance->atoms3lcd.loadingJpeg) {
-        if ((str[0] == jpegPacketHeader[0]) && (str[1] == jpegPacketHeader[1]) && (str[2] == jpegPacketHeader[2])) {
-            instance->atoms3lcd.currentJpegIndex += str.length() - 3;
-            // End loading if the entire JPEG is received
-            if (instance->atoms3lcd.currentJpegIndex >= instance->atoms3lcd.jpegLength) {
-                instance->atoms3lcd.loadingJpeg = false;
-                instance->atoms3lcd.readyJpeg = true;
-            }
-            return;
+    } else if (instance->atoms3lcd.loadingJpeg) {
+        size_t index = instance->atoms3lcd.currentJpegIndex;
+        size_t strLength = str.length();
+        // Continue loading JPEG data
+        if (index + strLength <= sizeof(instance->atoms3lcd.jpegBuf)) {
+          memcpy(instance->atoms3lcd.jpegBuf + index, str.c_str(), strLength);
+            instance->atoms3lcd.currentJpegIndex += strLength;
         } else {
-          // Stop loading if a wrong packet is received
-          instance->atoms3lcd.loadingJpeg = false;
+            instance->atoms3lcd.loadingJpeg = false;
+        }
+        if (instance->atoms3lcd.currentJpegIndex >= instance->atoms3lcd.jpegLength) {
+            instance->atoms3lcd.loadingJpeg = false;
+            instance->atoms3lcd.readyJpeg = true;
         }
+    } else {
+        instance->atoms3lcd.loadingJpeg = false;  // Reset if invalid packet received
     }
-    // Check for QR code header and store data if found
-    if (str.length() > 1 && str[0] == qrCodeHeader) {
-        uint8_t qrCodeLength = str[1];  // Assuming the length of the QR code data is in the second byte
+}
+
+void AtomS3I2C::handleQrCodePacket(const String& str) {
+    if (str.length() > 1) {
+        uint8_t qrCodeLength = static_cast<uint8_t>(str[1]);
         instance->atoms3lcd.qrCodeData = str.substring(2, 2 + qrCodeLength);
-        return;
     }
-    // Check for force mode change
-    if (str.length() > 1 && str[0] == forceModeHeader[0] && str[1] == forceModeHeader[1] && str[2] == forceModeHeader[2]) {
-      forcedMode = str.substring(3);
+}
+
+void AtomS3I2C::handleForceModePacket(const String& str) {
+    if (str.length() > 1) {
+        forcedMode = str.substring(1);
+    }
+}
+
+void AtomS3I2C::handleSelectedModePacket(const String& str) {
+    if (str.length() > 1) {
+        selectedModesStr = str.substring(1);
     }
-    instance->atoms3lcd.color_str = str;
 }
 
 void AtomS3I2C::stopReceiveEvent() {

diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.h b/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/atom_s3_i2c.h
@@ -6,6 +6,8 @@
 #include <atom_s3_lcd.h>
 #include <atom_s3_button.h>
 
+#include "packet.h"
+
 /**
  * @brief Handles I2C communication for AtomS3, including receiving and sending data via I2C bus.
  */
@@ -33,6 +35,8 @@ class AtomS3I2C {
    */
   bool checkTimeout();
 
+  int splitString(const String &input, char delimiter, String output[], int maxParts);
+
   void stopReceiveEvent();
   void startReceiveEvent();
   static void setRequestStr(const String &str);
@@ -52,6 +56,7 @@ class AtomS3I2C {
 #endif
 
   static String forcedMode;
+  static String selectedModesStr;
 
 private:
   bool receiveEventEnabled;
@@ -64,6 +69,7 @@ class AtomS3I2C {
   static constexpr uint8_t jpegPacketHeader[3] = { 0xFF, 0xD8, 0xEA }; /**< JPEG image packet header identifier. */
   static constexpr uint8_t qrCodeHeader = 0x02; /**< QR code packet header identifier. */
   static constexpr uint8_t forceModeHeader[3] = { 0xFF, 0xFE, 0xFD }; /**< Force mode packet header identifier. */
+  static constexpr uint8_t selectedModesHeader[3] = { 0xFC, 0xFB, 0xFA }; /**< Selected modes packet header identifier. */
 
   static String requestStr; /**< String to be sent on I2C request. */
 
@@ -78,6 +84,10 @@ class AtomS3I2C {
    * @param howMany Number of bytes received.
    */
   static void receiveEvent(int howMany);
+  static void handleJpegPacket(const String& str);
+  static void handleQrCodePacket(const String& str);
+  static void handleForceModePacket(const String& str);
+  static void handleSelectedModePacket(const String& str);
 
   /**
    * @brief Called when the master requests data from the I2C slave.

diff --git a/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/packet.h b/firmware/atom_s3_i2c_display/lib/atom_s3_i2c/packet.h
@@ -0,0 +1,11 @@
+enum PacketType : uint8_t {
+    TEXT = 0x00,
+    JPEG = 0x01,
+    QR_CODE = 0x02,
+    FORCE_MODE = 0x03,
+    SELECTED_MODE = 0x04,
+    DISPLAY_BATTERY_GRAPH_MODE = 0x05,
+    SERVO_CONTROL_MODE = 0x06,
+    PRESSURE_CONTROL_MODE = 0x07,
+    TEACHING_MODE = 0x08,
+};